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I.  INTRODUCTION 


During  the  last  century,  man  has  reached  out  over  ever-increasing  distances. 
Manmade  devices  have  been  sent  beyond  our  solar  system  and  to  the  deepest  points  of 
the  oceans.  These  recent  developments  have  focused  new  attention  on  an  existing 
problem:  how  to  accurately  track  long-range  devices  along  their  voyages  in  unknown 
environments.  This  problem  is  made  even  worse  when  only  passive  sensors  can  be  used. 
One  particular  problem  applicable  to  naval  technology  is  tracking  a  ship  by  lines  of 
bearing  obtained  by  passive  sensors.  A  powerful  method  of  dealing  with  this  problem, 
known  as  Kalman  filtering,  has  been  used  with  great  success  since  Kalman  and  Bucy 
[Refs.  1,2]  first  presented  its  principles  30  years  ago. 

This  report  further  develops  an  existing  Kalman  filter  to  which  a  fixed-interval 
smoothing  algorithm  has  been  added.  In  this  research,  we  examine  how  the  overall  ac¬ 
curacy  of  the  extended  Kalman  filter  is  affected  by  applying  a  noise  process  in  the 
smoothing  algorithm.  We  also  develop  a  new  maneuver  detection  technique  and  study 
how  the  filter  performs  when  using  it.  This  research  is  based  on  previous  work  done  by 
Lieutenant  Thomas  K.  Bennett  [Ref.  3]  and  Lieutenant  William  J.  Galinis  [Ref.  4], 
They  investigated  the  problems  of  two  ships  tracking  a  third  only  by  passive  radio  di¬ 
rection  finding  equipment. 

This  report  is  organized  into  six  major  sections.  The  first  section  is  this  introduc¬ 
tion,  which  serves  as  a  guide  to  approaching  this  report.  In  Chapter  II,  the  physical 
tracking  system  used  for  simulations  in  this  report  is  modeled.  Chapter  III  gives  the 
basic  principles  of  the  Kalman  filtering  and  fixed-interval  smoothing.  In  Chapter  IV, 
we  investigate  how  the  noise  process  in  the  smoothing  routine  and  a  new  maneuver  de¬ 
tection  technique  affect  the  accuracy  of  the  extended  Kalman  filter,  t  apters  V  and  VI 
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show  the  simulations  and  present  the  conclusions.  The  appendices  list  the  program 
codes  used  in  this  research. 
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PROBLEM  STATEMENT 


A.  THE  SYSTEM  MOL  L 

The  system  used  in  this  thesis  includes  two  sensors  and  one  target  ship.  A  two- 
dimensional  cartesian  coordinate  system  is  used,  in  which  the  positive  x  and  positive  y 
directions  correspond  to  East  and  North,  respectively.  The  target  and  sensor  ships  are 
both  free  to  move  throughout  this  coordinate  space.  For  simplicity,  the  following  as¬ 
sumptions  are  made  during  the  development  of  this  model:  [Ref.  4] 

•  The  effect  of  the  wind,  current  and  other  forces  on  the  ship  are  negligible. 

•  The  ocean  surface  is  considered  flat;  the  curvature  of  the  earth  is  neglected. 

•  Course  and  speed  inputs  are  taken  as  constants  (i.e.,  step  inputs). 

From  [Refs.  5:  p.  168,6:  pp.  12-13],  the  discrete-time,  state-space  representation  -f 
the  model  described  above  is 


A  ,  A  A 

xK+\  —<PKxK+  w  K 


(2.1) 


where 

=  state  estimate  vector, 
xK  *  state  vector, 

<j>x  ™  state  transition  matrix  and 
(ox  =  disturbance. 

A  state  vector  xK  is  defined  to  contain  the  minimum  number  of  the  elements  neces¬ 
sary  to  describe  the  target.  A  fourth  order  state  vector  for  this  model,  then,  consists  of 
the  position  and  velocity  of  the  target  in  both  x  and  y  directions. 
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(2.2) 


Next,  a  state  transition  matrix  4>K  is  chosen  to  fit  the  target  dynamics.  Since  the  target 
modeled  in  this  problem  moves  linearly  at  a  constant  velocity,  the  <t>K  matrix  is 


4>k  = 


i  r  o  o 


0  10  0 
0  0  1  T 
0  0  0  1 


where  T  is  the  observation  interval. 


The  unpredictable  accelerations  of  the  target  are  taken  into  account  using  the  noise 
vector  coK .  The  noise  vector  is  a  function  of  the  transition  matrix  Tz  and  the  acceleration 
matrix  aK: 


03  k  ~  ^K°K 


axs 

=  r* 

-ayic- 


where  the  noise  transition  matrix  T,  is  defined  as 


T2/2  0 


r„  = 


o  r2/2 
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Putting  Equations  (2.2)  through  (2.5)  into  Equation  (2.1),  the  final  state-space 
equation  for  the  system  modeled  in  this  problem  can  be  written  as 


1  T  0  o' 

XK 

1 - 

o 

l _ 

*K+ 1 

0  10  0 

*K 

T  0 

'a*K 

= 

+ 

y/c+i 

bi 

O 

o 

yK 

0  T212 

-ayx- 

0  0  0  1J 

1 

A 

- 1 

o 

_ 1 

(2.6) 


B.  THE  MEASUREMENT  MODEL 

For  linear  systems,  measurements  can  be  modeled  using  the  following  linear  meas 
urement  equation.  [Refs.  5:  p.  168,6:  pp.  12-13] 


A  rrA  ,  A 

ZK+ 1  -  H*K+ 1  +  Vk+  1 


(2.7) 


where 

z**,  =  measurements, 

H  —  observation  matrix, 
xK+l  =  state  estimate  vector  and 
Ajr+i  =  measurement  noise. 

Unfortunately,  many  real  systems  are  not  linear.  The  system  we  studied  in  this 
thesis  falls  in  this  category.  Although  this  system  has  a  linear  state-transition  equation, 
it  has  a  non-linear  measurement  equation,  since  the  measurements,  lines  of  bearings,  are 
non-linear  functions  of  the  system  states.  As  it  can  be  seen  from  the  geometry  of  the 
typical  scenario  in  Figure  1,  an  appropriate  model  with  measurement  noise  included  for 
the  non-linear  measurement  process  of  this  system  would  instead  be  [Ref.  3] 
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,N» 


Figure  1.  Typical  Tracking  Scenario 
where 


*  observed  lines  of  bearing  by  a  sensor  ship  n,  at  time  k, 
x,K  ,  y,K  =  position  of  the  target  ship  on  x,  y  axes,  at  time  k, 
x„K  ,  y  =  position  of  the  sensor  ship  n  on  x,  y  axes,  at  time  k  and 
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Ar  =  measurement  noise. 

Although  there  are  several  types  of  noise  which  disturb  the  measurements,  it  is  the 
atmospheric  noise  that  makes  the  major  contribution  in  the  frequency  range  of  interest 
in  this  study.  This  is  generally  a  non-white,  non-Gaussian  process.  However,  it  can  be 
considered  to  be  a  white  Gaussian  process  over  an  extended  period  of  time  in  order  to 
more  easily  implement  the  extended  Kalman  filter.  In  this  application,  a  white  noise 
model  is  used  for  the  study  of  noisy  cases. 
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III.  THEORY 


A.  KALMAN  FILTER 

The  Kalman  filter  removes  random  noise  from  the  state  estimates  of  a  system  by 
adding  a  weighted  error  term  to  the  predicted  state  estimates.  The  error  term  is  simply 
the  difference  between  the  filter's  prediction  of  the  measurement  and  the  observed  value 
of  that  measurement  at  a  particular  time.  The  weighting  factor,  also  called  the  filter 
gain,  is  based  on  the  predicted  covariance  of  error  between  estimates  and  observed  val¬ 
ues.  The  basic  operation  of  the  filter  can  be  described  in  several  steps: 

A  priori  estimates  of  the  state  xKjK  are  projected  in  time  to  some  predicted  state  es¬ 
timate  **„,/*,  and  the  predicted  error  covariance  of  these  estimates  is  calculated. 
The  filter  then  calculates  a  gain  vector  Gt+l,  based  on  the  predicted  error  covariance. 
As  mentioned  before,  the  error  is  the  difference  between  observed  and  predicted  meas¬ 
urements.  Next,  this  error  is  multiplied  by  the  filter  gain  and  the  result  is  added  to  the 
predicted  state  estimates  to  give  the  updated  estimate  ***,,*+,.  The  updated  value  of  er¬ 
ror  covariance  is  also  calculated. 

In  short,  the  Kalman  filter  is  a  linear,  minimum  variance  estimator.  A  block  dia¬ 
gram  of  the  filter  is  in  Figure  2.  A  more  detailed  explanation  of  the  filter's  operation 
will  be  given  later  in  this  chapter.  For  further  information  on  the  derivation  and  appli¬ 
cation  of  the  Kalman  filter,  the  reader  should  refer  to  [Refs.  7,8,9] 

B.  EXTENDED  KALMAN  FILTER 

The  Kalman  filter  explained  above  calculates  the  optimal  estimate  for  the  states  of 
linear  systems.  As  mentioned  before,  the  system  we  studied  in  this  thesis  has  a  linear 
state-transition  equation  and  non-linear  measurement  equation.  Therefore  it  is  not  lin- 
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A  Priori  Information 


-  Initial  State  Estimate 

-  Initial  Error  Covariance 

-  State  Transition  Matrix 

-  Measurement  Noise  Covariance 


Observations  z, 


■JC+I 


♦  t  t  t 


KALMAN  FILTER 


State  Estimate^™,**! 


Error  Estimated 


K*UK*l 


Figure  2.  Block  Diagram  of  the  Kalman  Filter. 


ear.  The  adaptation  of  the  Kalman  filter  to  a  non-linear  system  is  called  the  extended 
Kalman  filter. 

For  the  system  studied  in  this  thesis,  the  non-linear  measurement  equation  can  be 
defined  as: 


z*+ 1  _  H(xk+\)  +  tiK+ 1 


(3.1) 
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We  see  that  the  only  difference  between  this  equation  and  the  linear  measurement 
equation  (2.7)  is  the  observation  matrix  H.  The  H  matrix  is  now  a  function  of  the  sys¬ 
tem  states.  In  order  to  linearize  the  measurement  equation,  we  have  chosen  in  this  thesis 
to  expand  the  observation  matrix  H  in  a  Taylor  series  around  the  current  estimate  and 
then  to  use  only  the  first  order  term. 

The  following  linearized  form  of  the  measurement  equation  is  obtained  directly  from 
previous  work  on  this  subject.  Its  development  will  not  be  repeated  here  since  an  ex¬ 
cellent  derivation  of  it  is  presented  in  these  reports  [Refs.  3,4].  The  equations  are: 


Hk+  i  — 


A 

X, 


KM/K  Ar+i 
&K+ 1 


0  - 


XtK±l/K  XnK+l 


A  <5 

R2 


0 


K+\ 


(3.2) 


and 


«*+!-(& 


r*+i/jr 


-yn 


JT+l 


)2  + 


M  y 

lK+l/K  nK+l 


where 

x'k*uk  ’^jt+i/jc"  The  Pos^on  estimates  of  the  target  at  time  K+ 1,  based  on  the 
previous  value  at  time  K. 

•*"*+1  ».K.r+l  =  The  P0^011  °f  the  sensor  ship  n  at  time  K+  l. 

Once  the  measurement  process  is  linearized,  the  normal  linear  Kalman  filter 
equations  can  be  used  in  the  estimation  process.  The  following  Kalman  filter  equations, 
taken  from  [Ref.  5]  and  derived  in  [Refs.  5,10],  are: 


A  i  A 

XK+1/K  ~  <PXKIK 

(3.3) 

T 

P K+\!K  **  ^PkIK^  +  Qk+\ 

(3.4) 
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Gk+  1  ~  ?K+  1  / K Hk+  1 L  ^K+  \PK+\  IK 1  +  R] 

(3.5) 

xK+l IK+l  -xK+ll/C+  ^K+lLzK+l  ~  Hk+\xK+\IK~] 

(3.6) 

Rk+iIK+\  =  Gk+1Hk+11Pk+1iK 

(3.7) 

The  variables  are  defined  as  follows: 
xKJ.{/K  =  predicted  state  estimate, 
xK/K=  state  estimate  (state  vector), 

0  =  state  transation  matrix  given  by  equation  (2.3), 

PK+llK  ~  predicted  state  error  covariance, 

PK/K  =  state  error  covariance, 

Qk+\  ~  state  excitation  matrix, 

Gk+i  -  Kalman  gain  matrix, 

R  -  measurement  noise  covariance  matrix  and 
He+i  =  linearized  measurement  matrix  given  by  equation  (3.2). 

The  measurement  noise  covariance  matrix  R  is  a  indication  of  the  accuracy  of  the 
measurements  made.  This  matrix  is: 

R  —  \_PkN^\  (3-8) 

The  state  excitation  matrix  QK^  used  in  equation  (3.4)  represents  the  system  noise 
process.  This  term  is  a  measure  of  how  closely  the  system  model  actually  represents  the 
real  system  and  to  what  degree  the  system  is  affected  by  noise.  The  derivation  of  the 
Qk+i  matrix  will  be  studied  in  the  next  chapter. 

As  can  be  seen  from  equations  (3.3)  through  (3.7),  the  basic  operation  of  the  filter 
is  a  relatively  straightforward  recursive  process.  But  the  filter  must  be  initialized  before 
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processing  the  measurement  data.  When  the  filter  is  initialized,  no  prior  value  for  the 
state  estimate  xK/K  exists.  Therefore  the  value  of  the  first  observed  position  is  assigned 
to  it.  The  coordinates  of  observed  positions  can  be  calculated  from  the  two  lines  of 
bearings  by  using  the  following  equations: 


y2  tan(02)  +^i  tan(fl;)  +  x2  -  jt, 
tan(0t)  -  tan(02) 


i 


y\  tan(0[)  + 


(3.9) 


y2  tan(02)  +yt  tan(0,)  +  x2  —  x 


tan^)  —  tan(02) 


(3.10) 


Since  there  is  no  prior  velocity  information  available  at  the  moment  of  initialization, 
the  initial  velocity  estimate  is  taken  as  zero.  Figure  3  shows  the  initialization  procedure. 

Since  the  initial  state  estimates  will  have  some  error,  we  pick  some  starting  values 
for  the  errors  in  initial  position  and  velocity’  to  initialize  the  error  covariance  matrix. 
These  are  100  nautical  miles  (Nm)  in  position  and  0.5  Nm  per  minute  (i.e.,  30  kts)  for 
velocity  [Refs.  3,4].  The  error  covariance  matrix  can  now  be  initialized  as: 


10000 

0 


0 


0  0  0 

0.25  0  0 

0  10000  0 

0  0  0.25 


(3.11) 


Once  initialized,  the  filter  is  ready  to  process  the  measurements.  First,  the  state  es¬ 
timate  and  state  error  covariance  matrixes  are  projected  to  the  present  time  using  the  <f> 
matrix.  Next,  these  predicted  state  estimates  are  used  to  calculate  the  H  matrix.  Finally, 
the  Kalman  gains  are  calculated.  The  Kalman  gain  is  a  measure  of  where  the  filter's 
confidence  is  being  placed:  either  in  the  filter's  estimation  or  in  the  current  observation. 
As  is  se  in  equation  (3.5)  the  value  of  the  Kalman  gain  is  based  on  the  predicted  error 
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Figure  3.  The  Initialization  Procedure. 

covariance  matrix.  If  Pk+uk  is  large  the  Kalman  gain  will  approach  unity.  If  P is 
small,  the  gain  will  approach  zero  due  to  the  finite  value  of  the  measurement  noise 
covariance  R.  By  manipulating  equation  (3.6)  we  can  see  how  varying  the  Kalman  gain 
affects  the  process  of  updating  state  estimates. 

•*k-m/k+i  “  CJ-  +  ^ka-\zk+\  (3.12) 


13 


As  mentioned  before,  this  equation  shows  that  a  large  Kalman  gain  places  more 
weight  on  the  current  observation.  On  the  other  hand,  a  small  gain  causes  the  factor 
of  C /—  in  equation  (3.12)  to  approach  unity,  in  this  sense  placing  more  em¬ 

phasis  on  the  filter's  estimates.  As  can  be  seen  from  equation  (3.7),  the  factor  of 
C  /—  C/jc^ur+iD  is  used  to  update  state  error  covariance  matrix. 

C.  SMOOTHING  ALGORITHM 

Smoothing  is  a  non-real-time  data  process  used  to  reduce  error  in  state  estimates 
produced  by  a  Kalman  filter.  Let  time  K  be  within  the  time  interval  0  to  N,  so  that 
0  <.  K  ^  N.  A  Kalman  filter's  state  estimate  for  time  K,  denoted  by  xKIK,  is  based  only 
on  measurements  occuring  up  to  time  K.  But  the  smoothed  state  estimate  is  based  on 
the  measurements  that  occurred  over  the  entire  time  interval  0  to  N.  This  smoothed 
estimate  is  denoted  by  jcr,iV.  The  smoothed  error  covariance  at  time  K  is  represented  by 
Pjr/jy  This  quantity  has  no  impact  on  the  calculation  of  the  smoothed  estimate  xxiff  but 
it  is  an  indicator  of  how  well  the  smoothing  filter  is  working.  If  Px/M  <,  Px/x,  the 
smoothed  estimate  is  better  than  or  equal  to  its  filtered  estimate  except  for  the  last  data 
point  where  both  smoothed  and  filtered  estimates  are  equal.  The  smoothing  algorithm 
operates  backwards  in  time,  beginning  at  time  N  and  ending  at  time  zero.  Therefore, 
since  the  last  filtered  estimate  at  time  N  is  taken  as  the  first  smoothed  estimate,  PXj„ 
must  be  equal  to  P K/K  at  this  last  data  point.  This  can  be  seen  graphically  in  Figure  4. 

Meditch  [Ref.  5]  places  smoothed  estimates  into  three  classes: 

Fixed-Interval  smoothed  estimate  ,  denoted  by  xKlfl  where  K  *  0,  1, ....  N-l;  N  is  a 
positive  integer. 

Fixed-Point  smoothed  estimate  ,  denoted  by  xKU  where  J  =*  K  + 1,  K  +■  2, ....;  K  is  a 
fixed  integer. 

Fixed-Lag  smoothed  estimate  ,  denoted  by  xKIK+K  where  K  =  0,  1,  ....;  N  is  a  fixed 
positive  integer. 
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Mean  square  estimation  error 


Figure  4.  Advantage  of  the  Performing  Optimal  Smoothing. 

In  this  thesis  a  Fixed-interval  smoothing  Filter  is  used.  The  basic  block  diagram  oF 
this  filter  is  shown  in  Figure  5.  The  equations  to  implement  the  smoothing  algorithm 
are  obtained  The  equations  to  implement  the  smoothing  algorithm  are  obtained  From 
(ReF.  5:  pp.  216-224J  and  are  shown  below: 

“  Pkik^Pk+iik  (3-13) 

=  +  ^  *[•**+ 1 /tf~  xK+IIk]  (3-14) 


Figure  5.  Block  Diagram  of  the  Smoothing  Filter. 


in**  Pk/k  +  ^kLPk+ijn~  ^k+\ik1^k  (3.15) 

where 

Ak  =  smootliing  gain  matrix, 
xKlff  =  smoothed  estimate  at  time  K, 

PK,N  =  smoothed  error  covariance  at  time  K, 

xKtK  and  PKlK  =  state  estimate  and  error  covariance  stored  by  the  extended  Kalman 
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filter  routine  and 

xK+llK  and  PK+i,K  ~  predicted  state  estimate  and  predicted  error  covariance  stored 
by  the  extended  Kalman  filter  routine. 

Several  sources  were  helpful  in  understanding  these  equations.  [Refs.  7,8,1 1]  As  it 
can  be  seen  from  equation  (3.15),  the  smoothed  estimate  provided  by  a  fixed-interval 
smoothing  algorithm  is  simply  the  extended  Kalman  filter  estimate  adjusted  by  a 
weighted  error  term.  The  error  term  is  the  difference  between  the  smoothed  estimate 
calculated  for  the  previous  data  point  and  the  predicted  estimate  calculated  by  the  ex¬ 
tended  Kalman  filter.  It  is  also  clear  that  the  fixed-interval  smoothing  algorithm  uses 
the  values  of  xKIK  and  xK+UK  which  are  stored  in  the  Kalman  filter  routine  for  each  iter¬ 
ation.  Additionally  the  values  of  PK,K  and  PK+ttx:  must  be  provided  for  the  smoothing 
routine. 
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IV.  THE  NOISE  PROCESS  IN  FIXED-INTERVAL  SMOOTHING 


ALGORITHM. 


A.  GENERAL 

This  work  is  devoted  to  studying  the  effects  of  the  state  excitation  matrix  QK  in  the 
smoothing  algorithm.  To  accomplish  this,  the  magnitude  of  this  matrix  is  changed  dur¬ 
ing  the  assumed  maneuver  periods  and  the  effects  of  these  changes  on  the  smoothing 
algorithm's  accuracy  are  investigated.  Also,  a  new  maneuver  detection  technique  is  de¬ 
veloped  to  determine  the  maneuver  periods. 

B.  NOISE  PROCESS 

The  state  excitation  matrix  Qt  represents  the  system  noise  process.  This  matrix  is 
a  function  of  the  acceleration  matrix  aK  and  the  noise  transition  matrix  rK,  so  that 


Qk~ 


(4.1) 


where  <vx  is  given  by  equation  (2.4).  Substituting  equation  (2.4)  into  equation  (4.1),  we 
find 


’*[<] 

EloyXK2  £[<] 


(4.2) 


For  reference,  the  noise  transition  matrix  T*  is  given  by  equation  (2.5).  The  QK  matrix 
allows  for  any  random  target  maneuvers  and  also  serves  to  account  for  any  model  in¬ 
accuracies.  These  inaccuracies  are  the  differences  between  the  true  action  of  the  target 
and  its  motion  as  characterized  by  equation  (2.1).  QK  also  prevents  the  gain  matrix  GK 
from  approaching  zero  by  ensuring  some  uncertainty  in  the  predicted  state  error 
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covariance  matrix  P  r+i/Jf  By  substituting  equation  (2.5)  into,  the  equation  (4.2), 

equation  (4.2)  can  be  expanded  as  follows: 


£[<]>2 

f 


T£E‘W)7‘  |£C 

EZaxyK]T2 

f7WJ73 

T£KJr’ 


The  velocity  of  the  target  can  be  described  in  terms  of  its  linear  velocity  and  heading. 
From  Figure  1  on  page  6,  this  relationship  is  given  as 


v*  =  V,  sin  0, 

(4.4) 

Vy  =  vt  COS  0, 

(4.5) 

By  differentiating  equations  (4.3)  and  (4.4)  we  obtain  the  target's  acceleration  in  the 
x  and  y  directions: 

ax  —  v,  sin  0,  +  v(0,  cos  0f 


«x=vf^-  +  vx0f^- 


ajc=^ir  +  0r  vy 


and 


(4.6) 


ay  =  vt  cos  0,  —  v,0,  sin  0, 
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(4.7) 


.  vy  a  vx 

ay^vtX  +  v  r0f  — 

Vy 

ay  =  it  —  +  ®,vx 


The  noise  is  initially  described  by 


££*,]  =  £[©,]=0 

£[0r]  =  4, 


(4.8) 

(4.9) 


(4.10) 


By  squaring  equations  (4.6)  and  (4.7)  and  taking  the  expectations,  the  variances  of 
target's  accelerations,  a,  and  a,,  are: 


£[<]  =  [ 


(4.11) 


and 


*[4] 


(4.12) 


We  also  find  that  the  covariance  of  a,  and  a,  denoted  by  av  or  is 


=  £1  OyxKl 


if  )2-4, 


(4.13) 
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From  [Ref.  12],  the  position  of  the  target  assuming  speed  is  constant 


xk+\  =xk+vxxT  (4-14) 

yK+l=yK+vyJ  (4.15) 

and  the  position  of  the  target  assuming  acceleration  is  constant: 

**+1  ■  *i k+  vxJ  +  y  axJ2  (4- 1 6) 

yic+ 1  =yx  +  vyxT + y  “yj1  (4.17) 


By  comparing  the  equations  (4.14)  thorough  (4.17),  it  can  be  seen  that  the  expected 
position  errors  due  to  the  unknown  accelerations  of  the  target  can  be  defined  as 

(4.18) 

and 

£C7*+.]— (4.i9) 
The  variances  of  these  errors  are 


(4.20) 


(4.21) 


21 


By  comparing  equations  (4.20)  and  (4.21)  with  equation  (4.3),  we  see  that  these 
equations  are  equal  to  elements  (1,1)  and  (3,3)  of  the  QK  matrix.  Out  of  all  the  elements 
in  the  QK  matrix,  these  two  elements  have  the  greatest  effect  in  compensating  the  posi¬ 
tion  errors.  Since  it  is  most  important  to  compensate  the  error  on  the  axis  which  has 
the  maximum  error  variance,  the  algorithm  developed  determines  the  QK  matrix  using 
these  elements  for  the  magnitude  of  the  Qz  matrix.  This  algorithm  first  compares  the 
error  variances  on  the  x  and  y  axes,  a,  and  a},  to  determine  which  axis  has  the  greater 
error  variance.  If  c t,  >  a ,,  QK  matrix  becomes 

(4.22) 

where  I  is  the  unity  matrix  and 

If  a,  <  ay,  the  Qx  matrix  is 

Qk  ~  (4-23) 


where 


C.  THE  STATE  EXCITATION  MATRIX  IN  THE  FIXED-INTERVAL 
SMOOTHING  ALGORITHM 

As  mentioned  before,  the  fixed-interval  smoothing  filter  uses  as  input  the  state  esti¬ 
mates  and  error  covariances  calculated  by  the  forward-time  Kalman  filter.  But  in  order 
to  see  the  effects  of  the  state  excitation  matrix  QK  in  the  smoothing  algorithm,  the  pre¬ 
dicted  error  covariance  matrix  PK+UK  is  recalculated  in  the  smoothing  routine.  The  pre- 
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dieted  state  estimates  xK+ljK  are  also  recalculated  in  the  smoothing  routine.  By 
recalculating  these  matrices,  we  attempted  to  get  a  feeling  for  the  expected  magnitude 
of  the  smoothing  error.  The  intent  was  to  enable  the  the  filter  to  carry  along  its  own 
error  analysis.  The  new  system  of  the  recursive  equations  for  the  fixed-interval 
smoothing  becomes:  [Refs.  4,13] 


A  ,  A 

XK+\IK  ~  <PXKIK 

(4.24) 

T 

P  K+\/K  —  $ PkIK $  +  Qk 

(4.25) 

AK  —  PjqK $  TP K+\IK 

(4.26) 

XK!N  ~  XK\K  +  AkLXK+\/N  ~  ^AM-t/Ar] 

(4.27) 

PkIN  ~  PkIK  +  Ak[Pk+IIN  ~  pk+\ik^ak 

(4.28) 

As  seen  from  equation  (4.26),  the  smoothing  filter  gains  are  a  function  of  the  error 
covariance.  As  the  predicted  error  increases,  the  smoothing  gains  decrease  due  to  the 
inverse  relationship  between  smoothing  gains  and  the  predicted  error  covariance  matrix. 
In  this  way  the  smoothing  filter  can  compensate  for  a  large  expected  error  by  placing 
more  emphasis  on  the  Kalman  filter  estimates.  By  substituting  equation  (4.24)  into 
equation  (4.27),  we  obtain 

XKjN  ~  XKIK  [■**+! /V  -  <t>XK/K2 

xKjS  ~  —  AK$]XKIK  +  A KXK+\IN  (4.28) 

Equation  (4.28)  shows  that  a  small  Ae  causes  the  factor  of  £/  —  A to  approach 
unity,  thereby  placing  more  emphasis  on  the  forward-time  Kalman  filter  estimates  xKjK  . 
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We  can  exploit  this  behavior  of  the  smoothing  filter  and  use  it  to  adapt  the  smoothing 
filter  to  detected  target  maneuvers. 

D.  MANEUVER  DETECTION 

Should  the  target  maneuver  during  the  tracking  process,  the  filtered  estimates  tend 
to  diverge  from  the  true  estimates.  This  introduces  error  into  the  state  estimates. 
Therefore  a  procedure  must  be  developed  to  detect  the  target's  maneuvers.  This  can  be 
accomplished  by  monitoring  the  filter  residual  process. 

The  residual  process  of  the  extended  Kalman  filter  is  taken  as  the  difference  between 
the  observed  position  and  the  filter's  predicted  position  estimates.  This  process  can  be 
defined  as 


2  AT  —  XK/K- 1 1 


(4.29) 


The  maneuver  detection  technique  implemented  calculates  the  residual  value  for  each 
observation  and  compares  this  to  the  two  maneuver  gates.  The  gates  are  defined  as 
three  times  and  eight  times  the  predicted  standard  deviation.  Some  of  the  principles 
underlying  this  technique  are  presented  in  [Ref.  14].  To  define  the  predicted  standard 
deviation,  error  ellipse  equations  are  used.  More  detailed  information  about  error  el¬ 
lipses  can  be  found  in  [Ref.  6:  pp.  17-18].  These  equations  are: 


2  +  cov(xy) 

<Tjc'“  2  +  sin  2d 


(4.30) 


2  a\  +  <*y  cov{xy) 
2  sin  2d 


(4.31) 


where 

o\  and  <tJ  *  variances  in  the  original  cartesian  coordinate  system, 
a\.  and  a],  -  variances  along  the  major  and  minor  axis  oriented  by 
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6 


2cov(jg;) 


(4.32) 


By  taking  the  square  roots  of  equations  (4.30)  and  (4.31),  the  standard  deviations 
on  the  x'  and  y'  axes  of  the  error  ellipse  are 


Or*  - 


a\  +  cov(xy) 


sin  26 


(4.33) 


and 
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2  2 

°x  +  ay  cov(xy) 
2  sin  26 


(4.34) 


The  maneuver  detection  algorithm  compares  the  two  standard  deviations  which  are 
represented  by  the  lengths  of  the  x'  and  y'  axes  of  the  error  ellipse  shown  in  Figure  6. 
It  selects  the  larger  one  as  a  predicted  standard  deviation,  allowing  the  gates  to  take  on 
the  following  values: 


LO  WERJ3A  TE  =  3<rx 


and 


UPPER J3  ATE  =  8<7* 

where  aK  is  the  larger  of  the  two  standard  deviations,  ax.  or  ar. 

The  reason  for  choosing  the  value  of  3 aK  for  the  lower  gate  is  well  explained  in  [Ref. 
14].  The  value  of  the  upper  gate,  known  as  a  "Glitch"  gate,  is  dependent  on  the  opera¬ 
tional  characteristics  of  the  target.  This  gate  rejects  motions  that  the  target  could  not 
possibly  make.  In  our  problem,  extremely  high  linear  or  tangential  accelerations  are 
examples  of  such  behavior.  When  the  residual  exceeds  this  gate,  the  filter  recognizes 
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Figure  6.  Error  Ellipse 


that  this  motion  is  impossible  for  the  given  target  and  so  must  be  due  to  noise.  The 
value  of  %rrK  gave  the  best  results  in  this  application. 

For  each  observation,  the  calculated  residual  is  compared  to  the  two  gates  by  the 
maneuver  detection  algorithm  in  the  extended  Kalman  filter  routine.  If  the  residual  is 


less  than  the  value  of  the  lower  gate,  the  filter  continues  on  and  processes  the  next  ob¬ 
servation.  If  the  residual  is  larger  than  the  value  of  the  upper  gate,  the  filter  ignores  that 
observation  by  setting  filter  gains  equal  to  zero,  thereby  making  the  state  estimates  equal 
to  the  predicted  estimates, 

xK+llK+lssxK+\!K  (4.35) 

This  procedure  will  work  well  for  isolated  bad  observations.  However,  if  there  are  se¬ 
veral  consecutive  bad  observations,  the  filter  can  conceivably  lose  track  of  the  target  as 
the  filter's  state  estimates  diverge  more  and  more  away  from  the  actual  target  states. 
To  remedy  this,  the  extended  Kalman  filter  sets  the  filter  gains  equal  to  zero  only  for  the 
first  of  two  consecutive  bad  observations  but  uses  non  zero  gain  for  the  second.  If  the 
residuals  of  the  two  consecutive  observations  are  in  the  zone  between  the  two  maneuver 
gates,  shown  as  concentric  circles  in  Figure  7,  a  maneuver  is  detected  and  compensation 
algorithm  begins.  The  value  of  two  provides  a  trade-ofF  between  fast  response  and  low 
false  alarm  rates. 

The  maneuver  detection  algorithm  does  not  run  a  second  time  in  the  fixed-interval 
smoothing  routine,  since  it  can  use  the  maneuver  times  detected  in  the  extended  Kalman 
filter  with  no  loss  of  accuracy.  Additionally,  the  fixed-interval  smoothing  algorithm 
"backs  up"  and  considers  the  first  point  ignored  by  the  Kalman  filter  as  a  maneuver 
point,  since  it  knows  that  if  the  maneuver  is  detected  at  some  observation  time  in  the 
Kalman  filter  routine,  it  must  have  started  one  observation  earlier. 

During  the  compensation,  the  state  excitation  matrix  QK  is  increased  by  multiplying 
the  coefficients  along  the  main  diagonal  by  a  factor  of  2.0.  These  coefficients  account 
for  random  course  and  speed  changes  of  the  target.  As  the  QK  matrix  is  increased,  the 
predicted  error  covariance  P Jt+l/K  is  also  increased  because  of  the  direct  effect  of  the  QK 
matrix  on  th  nagnitude  of  the  predicted  error  covariance.  And,  as  the  PK+UK  matrix 
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♦  PREDICTED  ESTIMATE 

•  PREVIOUS  OBSERVATION 
O  CURRENT  OBSERVATION 

A.  NO  MANEUVER 

B.  MANEUVER 

C.  BAD  OBSERVATION 


Figure  7.  Diagramming  the  Maneuver  Detection  Technique 

increases,  the  outputs  of  both  the  extended  Kalman  filter  and  the  smoothing  filter  are 
afiected  as  explained  before.  The  multiplicative  constant  of  2.0  was  found  by  trial  and 


V.  COMPUTER  SIMULATIONS 


A.  GENERAL 

The  SHIPTRACK.FOR  extended  Kalman  filter  algorithm  was  first  implemented  in 
[Ref.  3]  on  an  Apple  Macintosh  Plus  microcomputer.  In  [Ref.  4]  the  fixed-interval 
smoothing  algorithm  was  added,  the  new  algorithm  was  named  SHIPSM.FOR  this  al¬ 
gorithm  was  adapted  to  run  on  an  IBM  PC.  This  research  takes  the  program  one  step 
further  by  adding  new  algorithms  to  detect  maneuvers  and  to  derive  the  state  excitation 
matrix  Qx.  A  new  program  SHIPMANE  is  used  in  the  following  manner  for  the  simu¬ 
lations. 

The  raw  data  required  by  the  SHIPMANE.FOR  is  generated  by  RAWDATA.FOR. 
This  program  is  modified  from  the  program  TRACK.FOR  used  in  [Ref.  3].  Our  inten¬ 
tion  was  to  make  the  target  follow  a  circular  track  during  the  maneuver  period  rather 
than  make  a  sharp  turn.  Program  RAWDATA.FOR  asks  the  user  for  the  initial  posi¬ 
tions,  speeds  and  courses  of  the  target  and  the  tracking  ships,  the  total  tracking  time  and 
the  observation  interval.  It  also  requests  the  desired  maneuver  period  and  any  speed  and 
course  changes  of  the  target  during  this  period.  The  outputs  consist  of  noisy  or  noise 
free  bearings  from  each  tracking  ship  to  the  target,  the  updated  positions  of  all  the  ves¬ 
sels  and  the  time  of  the  observation  are  stored  in  the  file  called  TRKDATA.DAT. 

The  program  SHIPMANE.FOR  reads  and  processes  the  data  stored  in 
TRKDATA.DAT.  The  outputs  of  this  program  are  mainly  stored  in  three  files.  The 
first  file  FILDATA.DAT  stores  the  results  of  the  extended  Kalman  filter  portion  of  the 
program  SHIPMANE.FOR  while  the  fixed-interval  smoothing  results  are  included  in 
the  second  file  SMDATA.DAT.  The  results  of  the  maneuver  detection  algorithm  are 
stored  in  the  third  file  MANEUDATA.DAT  during  the  process  of  the  extended  Kalman 
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filter  portion  of  the  SHIPMANE.FOR.  Additionally,  a  fourth  file  TRUDATA  .DAT, 
is  created  for  graphic  purposes,  and  consists  of  the  actual  positions  (tracks)  of  the  target. 
Although  this  file  is  useful  for  the  purposes  of  this  thesis,  in  real  world  tracking  problem 
this  information  would  seldom,  if  ever,  be  available.  In  this  thesis  the  terms  "real"  or 
"actual",  when  applied  to  tracks  or  maneuvers,  refer  to  the  data  contained  in  this  file. 
"Assumed"  tracks  or  maneuvers  refer  to  what  is  detected  by  the  extended  Kalman  filter 
or  the  smoothing  routine. 

The  MATLAB  graphic  routines  are  used  to  obtain  the  graphical  representations  of 
the  data  included  in  the  output  files  of  the  SHIPMANE.FOR.  Five  graphic  outputs  are 
obtained  for  each  simulation  case  except  for  the  third  case,  which  has  only  two.  For  all 
cases  except  the  third,  the  first  graph  is  a  geographic  plot  which  show  extended  Kalman 
filtered  track  versus  the  the  actual  and  observed  target  tracks.  The  second  graph  com¬ 
pares  the  track  resulting  from  the  fixed-interval  smoothing  with  the  actual  and  observed 
target  tracks.  The  third  graph  is  the  time  plot  showing  the  filtered,  smoothed  and  ob¬ 
served  position  errors.  The  fourth  is  also  a  time  plot  and  shows  the  residuals  for  each 
observation  along  with  the  threshold  values  of  the  upper  and  lower  maneuver  detection 
gates.  In  the  third  simulation  case,  only  this  graph  is  included.  The  fifth  graph  gives  the 
overall  results  for  each  case.  Due  to  the  limited  number  of  variables  which  can  be  used 
in  the  single  MATLAB  graphic  package,  the  true  track  of  the  target  was  shown  as  a  line 
without  each  observation  point  being  shown. 

Although  both  of  the  programs  SHIPMANE.FOR  and  RAWDATA.FOR  can  eas¬ 
ily  be  modified  for  multi-bearing  measurements,  the  simulation  cases  used  only  two 
bearings  per  observation  as  measurements,  one  from  each  tracking  ship.  The  set  of  the 

simulations  studied  in  this  chapter  consists  of  following  cases: 

•  Case  #1:  60°  maneuver  toward  tracking  ships,  with  noiseless  measurements. 

•  Case  #2:  60°  maneuver  away  from  tracking  ships,  with  noiseless  measurements. 
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•  Case  #3:  Test  for  the  maneuver  detection  algorithm.  (1)  Case  #1  with  different 
maneuver  period.  (2)  Case  #2  with  different  maneuver  period. 

•  Case  #4:  60°  maneuver  toward  tracking  ships,  with  noisy  measurements. 

•  Case  #5:  120°  maneuver  toward  tracking  ships,  with  noisy  measurements. 

•  Case  #6:  60°maneuver  away  from  tracking  ships,  with  noisy  measurements. 

•  Case  #7:  120°  maneuver  away  from  tracking  ships,  with  noisy  measurements. 

In  all  cases,  the  target  ship  starts  at  the  position  (-75,150).  The  initial  course  of  the 
target  is  090°  and  the  initial  speed  of  the  target  is  15  knots  for  each  case.  The  speed  of 
the  target  is  held  constant  throughout  the  simulation  cases.  The  initial  positions  of  the 
tracking  ships  are  (-40,0)  and  (-60,0),  and  courses  and  speeds  are  030°  and  10  knots  for 
each  case.  The  speeds  and  courses  of  the  tracking  ships  are  also  held  constant.  The 
observation  period  is  30  minutes  and  all  cases  run  for  450  minutes. 

The  success  of  the  algorithm  can  be  expressed  by  the  percentage  improvement  be¬ 
tween  the  total  error  in  observed  positions  and  the  total  errors  in  the  filtered  estimates 
and  smoothed  estimates,  respectively.  This  percentage  indicates  of  how  much  the  ex¬ 
tended  Kalman  filtering  and  the  fixed-interval  smoothing  improve  the  position  accuracy 
over  the  observations.  For  the  extended  Kalman  filter,  this  percentage  is  simply  the  ra¬ 
tio  between  the  the  total  error  in  the  observed  positions  and  the  total  error  in  the  filtered 
estimates  throughout  the  simulation  case  or  time  period  of  interest.  In  some  cases  this 
was  recalculated  specifically  for  maneuver  periods.  The  percentage  improvement  due  to 
the  smoothing  was  similarly  the  ratio  between  the  total  error  in  the  observed  position 
estimates  and  the  total  error  in  the  smoothed  position  estimates.  Also,  the  average  po¬ 
sition  errors  due  to  the  extended  Kalman  filter  and  the  fixed-interval  smoothing  algo¬ 
rithm  are  given  for  the  different  cases.  The  average  position  error  due  to  the  extended 
Kalman  filter  is  calculated  by  summing  the  position  errors  of  the  filtered  position  esti¬ 
mates  and  then  dividing  by  the  total  number  of  observations.  The  average  position  error 
due  to  the  smoothing  routine  is  also  found  by  summing  the  position  errors  of  smoothed 
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estimates  over  the  entire  simulation  (or  in  some  cases,  over  the  number  of  observations 
of  interest)  and  dividing  by  the  number  of  observations.  The  average  position  errors 
show  how  well  the  extended  Kalman  filter  and  fixed-interval  smoothing  algorithm  work 
for  a  particular  simulation  case. 
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B.  CASE  #1 


The  target  is  steaming  due  east  at  15  knots  at  the  beginning  of  this  case.  Between 
time  equals  150  minutes  and  and  time  equals  300  minutes,  it  makes  a  60°  course  change 
toward  the  advancing  tracking  ships  on  a  circular  track.  The  results  for  the  filtering  and 
smoothing  are  shown  in  Figure  8  and  Figure  9.  Since  there  is  no  noise  in  the  meas¬ 
urements,  the  observed  track  equals  the  true  track.  Although  the  measurements  are  not 
noisy,  the  filter  estimates  diverge  very  slightly  for  the  first  several  observations.  This 
initial  error,  shown  in  Figure  10,  is  due  to  the  inaccuracy  of  the  initial  state  estimates. 
This  inaccuracy  also  causes  the  high  values  for  the  upper  and  lower  maneuver  gates  for 
the  first  few  observations,  as  can  be  seen  in  Figure  11.  When  the  target  starts  its  turn 
at  time  equals  150,  the  tracking  error  begins  to  increase.  It  decreases,  however,  as  the 
filter  regains  the  target  track  and  it  reaches  zero  one  observation  after  the  target  finishes 
its  maneuver. 

The  fixed-interval  smoothing  algorithm  improves  the  position  accuracy  over  the  ex¬ 
tended  Kalman  filter  by  an  average  of  35%  during  the  real  maneuver  period,  between 
time  equals  150  and  equals  300  minutes,  and  22%  during  the  overall  simulation. 

As  can  be  seen  in  Figure  11,  the  residuals  appear  between  the  upper  and  lower 
maneuver  gates  for  time  equals  240,270  and  300.  Since  two  consecutive  residuals  be¬ 
tween  the  upper  and  lower  gate  values  are  necessary  for  the  maneuver  to  be  detected, 
the  extended  Kalman  filter  recognizes  times  270  and  300  as  a  maneuver  period.  Time 
240  is  ignored  by  the  Kalman  filter.  The  smoothing  algorithm,  however,  does  not  ignore 
time  240,  since  it  knows  that  if  the  maneuver  was  detected  at  time  270,  it  must  have 
begun  at  time  240.  Therefore,  the  maneuver  period  for  the  fixed-interval  smoothing  al¬ 
gorithm  is  taken  as  times  240,  270  and  300.  The  overall  results  of  this  case  can  be  seen 
in  Figure  12. 
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!.  The  Results  of  the  Kalman  Filter  Tracking  for  Case  #1 
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Figure  9.  The  Results  of  the  Fixed-Interval  Smoothing  for  Case  #1 
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Figure  10.  The  Position  Errors  for  Case  //I 
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Figure  1 1.  The  Results  of  the  Maneuver  Detection  Algorithm  for  Case  #1 
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Figure  12.  The  Overall  Results  for  Case  #  1 
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C.  CASE  #2 


In  this  case,  the  target  makes  a  60°  maneuver  away  from  the  two  tracking  ships. 
The  target's  initial  course  is  090°  at  15  knots.  Between  times  150  and  300,  it  turns 
northeast  to  a  new  course  ,  030°,  on  a  circular  track.  Again,  the  observed  and  true 
tracks  are  the  same  due  to  the  lack  of  measurement  noise.  T  results  of  filtering  are  in 
Figure  13  and  the  results  of  the  smoothing  routine  are  in  Figure  14.  The  initial  error 
in  Figure  15  and  the  high  maneuver  gate  values  for  the  first  few  observations  in 
Figure  16  are  again  due  to  the  error  in  the  initial  estimates. 

The  filter  error  starts  to  increase  when  the  target  begins  to  maneuver  at  time  150. 
When  the  target  completes  its  maneuver,  the  error  approaches  zero.  Since  the  target 
starts  to  pull  away  from  the  tracking  ships,  the  filter  error  reaches  zero  later  than  it  did 
in  the  previous  case.  From  Figure  15,  we  can  see  how  the  fixed-interval  smoothing 
routine  improves  the  filter's  estimate.  The  smoothing  algorithm  decreases  the  position 
error  of  the  extended  Kalman  filter  by  an  average  of  22%  for  the  overall  case  and  by  an 
average  of  38%  for  the  real  maneuver  period  between  150  and  300  minutes. 

From  Figure  16,  the  residuals  at  times  240,  270  and  300  are  in  the  maneuver  zone. 
The  maneuver  period  is  detected  for  times  270  and  300  for  the  extended  Kalman  filter 
and  for  times  240,  270  and  300  for  the  smoothing  filter.  The  final  results  of  this  case  are 
in  Figure  17. 
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Figure  14.  The  Results  of  the  Fixed-Interval  Smoothing  for  Case  #2 
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Figure  15.  The  Position  Errors  for  Case  #2 
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Figure  16.  The  Results  of  the  Maneuver  Detection  Algorithm  for  Case  #  I 
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Figure  17.  The  Overall  Results  for  Case  #2 
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D.  CASE  #3 


In  this  case,  the  two  previous  cases  are  tried  with  different  maneuver  periods  in  order 
to  test  the  performance  of  the  maneuver  detection  algorithm.  Therefore  only  the  figures 
which  show  the  results  of  the  maneuver  detection  algorithm  are  included. 

1.  Case  #1  With  Different  Maneuver  Period 

In  this  part  of  the  case,  Case  #1  is  again  tried  with  the  new  maneuver  period 
from  time  270  to  time  390.  From  Figure  IB,  it  can  be  seen  that  the  residuals  at  times 
300,  330,  360  and  390  are  between  the  upper  and  lower  gates.  The  maneuver  period  is 
between  330  and  390  minutes  for  the  extended  Kalman  filter  algorithm  and  between 
times  300  and  390  for  the  fixed-interval  smoothing  algorithm. 

2.  Case  #2  With  Different  Maneuver  Period 

In  this  part,  Case  #2  with  a  new  maneuver  period,  this  time  between  90  and  270 
minutes,  is  simulated.  In  Figure  19,  the  residuals  are  in  the  maneuver  detection  zone 
at  180,  210,  240  and  270  minutes.  In  the  extended  Kalman  filter  routine,  the  maneuver 
detection  algorithm  detects  the  maneuver  at  210,  240  and  270  minutes.  For  the  fixed- 
interval  smoothing  algorithm,  the  maneuver  period  begins  at  time  180  and  ends  after 
time  270. 


45 


NAUTICAL  MILES 


LOWER  GATE-o  4  UPPER  GATE-+  A  RES1DUAU* 


Figure  18.  Tlie  Results  of  the  Maneuver  Detection  Algorithm  for  Case  #3-(l) 
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Figure  19.  The  Results  of  the  Maneuver  Detection  Algorithm  for  Case  #3-(2) 


E.  CASE  #4 


This  case  is  the  same  as  Case  #1  except  noise  is  added  to  the  measurements.  The 
filtering  and  smoothing  results  for  this  case  are  shown  in  Figure  20  and  Figure  21.  At 
the  beginning  of  the  tracking  problem,  the  error  is  high.  However,  after  the  target  ma¬ 
neuvers,  the  vessels  close  each  other  and  the  position  error  decreases  rapidly. 

From  Figure  23,  it  is  seen  that  the  residuals  are  between  the  maneuver  gates  at 
times  180  through  330.  The  maneuver  period  for  the  extended  Kalman  filter  is  from  210 
to  330  minutes.  During  this  period  the  improvement  in  position  error  due  to  the  Kalman 
filter  is  32%.  The  maneuver  period  for  the  smoothing  filter  is  between  times  180  and 
330,  and  the  position  error  improvement  due  to  the  smoothing  filter  is  78%. 

It  is  also  seen  that  the  maneuver  detection  algorithm  recognizes  a  bad  observation 
at  time  60.  As  Figure  20  shows,  the  extended  Kalman  filter  estimates  for  this  point 
appear  to  be  only  the  projections  of  the  previous  estimates  in  time.  Therefore  the  posi¬ 
tion  error  of  the  extended  Kalman  filter  for  this  point  is  145%  worse  than  the  observed 
position  error,  while  the  smoothed  position  error  is  only  4%  worse.  The  average  posi¬ 
tion  error  is  3.8  Nm  for  the  extended  Kalman  filter  and  2.5  Nm  for  the  smoothing  filter 
over  the  entire  tracking  period.  The  overall  results  for  this  case  are  in  Figure  24. 
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Figure  21.  Tlie  Results  of  the  Fixed-Interval  Smoothing  for  Case  # 4 
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Figure  22.  The  Position  Errors  for  Case  #4 
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Figure  23.  The  Results  of  the  Maneuver  Detection  Algorithm  for  Case  #4 
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F.  CASE  #5 


This  case  includes  a  120°  turn  which,  if  undetected,  will  cause  an  unacceptably  high 
error.  The  target  turns  to  a  new  course  of  210°  by  following  a  circular  track  between 
times  150  and  300.  Again,  the  observed  position  error  decreases  rapidly  after  the  target 
completes  its  maneuver,  since  all  the  vessels  start  to  close  each  other.  The  extended 
Kalman-filtered  and  smoothed  results  can  be  seen  in  Figure  25  and  Figure  26. 

The  observed,  filtered  and  smoothed  position  errors  are  shown  in  Figure  27.  The 
extended  Kalman  filter  improves  the  position  accuracy  by  46%  over  the  observed  posi¬ 
tion  errors  for  the  entire  tracking  period,  and  the  improvement  due  to  smoothing  is  73% 
over  the  same  period.  The  maximum  filtered  position  error  is  around  6  Nm  except  at 
time  zero,  while  the  average  filtered  error  is  3  Nm.  The  maximum  smoothed  position 
error  is  4  Nm  and  the  average  smoothed  position  error  is  1.5  Nm. 

The  maneuver  period  for  the  extended  Kalman  filter  is  from  180  through  330  min¬ 
utes,  during  which  the  improvement  due  to  the  extended  Kalman  filter  is  46%.  The 
maneuver  period  for  the  fixed-interval  smoothing  is  between  times  150  and  330,  and  the 
position  accuracy  is  55%  over  the  observed  position  errors  for  the  maneuver  period 
alone.  The  observation  at  time  equals  120  is  recognized  as  a  bad  observation.  The 
overall  results  are  in  Figure  29. 
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Figure  25.  Hie  Results  of  the  Kalman  Filter  Tracking  for  Case  #5 
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Figure  26.  The  Results  of  the  Fixed- Interval  Smoothing  for  Case  #5 
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Figure  27.  The  Position  Errors  for  Case  # 5 
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Figure  28.  The  Results  of  the  Maneuver  Detection  Algorithm  for  Case  #5 
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G.  CASE  #6 


This  case  is  the  same  as  Case  #2,  with  the  addition  of  noise  to  the  measurements. 
The  results  of  the  extended  Kalman  filtering  and  smoothing  are  in  Figure  30  and 
Figure  31. 

From  Figure  32,  the  maximum  Kalman  filter  position  error  is  8  Nm  at  time  equals 
30  while  the  average  filtered  position  is  3.3  Nm.  The  maximum  errors  are  3.5  Nm  at 
time  420  and  6.5  Nm  at  time  450,  which  the  filtered  and  smoothed  errors  had  to  be  same, 
and  the  average  error  is  2.1  Nm  for  the  smoothed  errors.  This  case  shows  the  general 
improvement  in  the  filtered  and  smoothed  estimates.  The  position  accuracy  increases 
by  47%  with  the  extended  Kalman  filter  and  by  65%  with  the  fixed-interval  smoothing 
filter. 

As  seen  in  Figure  33,  the  residual  values  are  in  the  maneuver  zone  at  times  60,  180, 
210,  240,  270  and  330  minutes.  No  maneuver  detection  occurs  at  times  60  and  330,  since 
the  residuals  immediately  following  these  times  are  out  of  the  maneuver  zone.  The  ma¬ 
neuver  periods  are  detected  from  210  to  270  minutes  for  the  extended  Kalman  filter  and 
from  180  to  270  minutes  for  the  smoothing  filter.  The  improvement  in  the  accuracy  of 
the  position  estimates  is  49%  due  to  the  extended  Kalman  filter  and  60%  due  to  the 
smoothing  filter. 

Figure  33  also  shows  that  the  maneuver  detection  algorithm  recognizes  the  obser¬ 
vations  at  times  120,  300  and  420  as  bad  observations.  As  can  be  seen  from  Figure  30, 
the  filtered  positions  are  the  projections  of  the  previous  estimates  in  time  with  no  noise 
adaptation  being  made.  For  each  of  these  times  the  filtered  estimates  are  more  accurate 
than  the  observed  estimates  and  the  smoothed  estimates  are  the  most  accurate  of  all. 
The  average  improvement  in  the  position  estimate  for  these  three  observations  is  56% 
for  the  extended  Kalman  filter  and  83%  for  the  smoothing  filter.  Figure  34  shows  the 
overall  tracking  and  smoothing  results  for  this  case. 
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Figure  32.  The  Position  Errors  for  Case  #6 
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Figure  33.  The  Results  of  the  Maneuver  Detection  Algorithm  for  Case  #6 
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Figure  34.  The  Overall  Results  for  Case  #6 
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H.  CASE  #7 


This  case  depicts  a  1208  target  maneuver  away  from  the  tracking  ships.  The  filtered 
and  smoothed  tracks  are  in  Figure  35  and  Figure  36. 

The  observed,  filtered  and  smoothed  position  errors  are  in  Figure  37.  The  accuracy 
of  the  position  estimates  is  increased  by  47%  with  the  extended  Kalman  filter  and  by 
65%  with  the  fixed-interval  smoothing  throughout  the  entire  tracking  period.  The  av¬ 
erage  position  error  due  to  the  Kalman  filter  is  5.4  Nm  while  the  average  position  error 
of  the  smoothed  estimates  is  2.3  Nm.  The  smoothed  error  is  always  less  than  5  Nm  with 
the  exception  of  the  last  observation  time  (i.e.  time  450)  where  the  error  is  5.4  Nm. 

From  Figure  38,  the  maneuver  period  is  detected  as  times  180,  210,  240  and  270  for 
the  extended  Kalman  filter  and  as  times  150,  180,  210,  240  and  270  for  the  smoothing 
algorithm.  During  these  periods,  the  accuracy  in  the  position  estimates  is  improved  by 
42%  with  the  Kalman  filter  and  by  72%  with  the  smoothing.  The  observations  of  times 
90  and  300  are  recognized  as  bad  observations.  The  Kalman  filter  improves  the  position 
accuracy  by  an  average  of  68%  and  the  average  improvement  due  to  the  smoothing 
routine  is  90%  for  these  two  points.  The  overall  results  for  this  case  are  shown  in 
Figure  39. 
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NAUTICAL  MILES 


Figure  37.  The  Position  Errors  for  Case  #7 
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Figure  38.  The  Results  of  the  Maneuver  Detection  Algorithm  for  Case  #7 


70 


Y  POSmON 


Figure  39.  The  Overall  Results  for  Case  #7 


VI.  CONCLUSIONS 


We  have  tried  to  improve  the  accuracy  of  the  extended  Kalman  filter  with  a  fixed- 
interval  smoothing  routine  by  implementing  a  new  maneuver  detection  algorithm. 
Whereas  maneuver  detection  algorithms  are  normally  applied  only  to  the  extended 
Kalman  filter,  we  apply  this  algorithm  both  to  the  extended  Kalman  filter  and  to  the 
fixed-interval  routine  to  adapt  them  both  to  unpredicted  maneuvers  of  the  target.  We 
studied  the  effects  of  varying  the  state  excitation  matrix  QK  in  the  fixed-interval 
smoothing  during  the  assumed  maneuver  periods.  Several  simulation  cases  were  run  and 
analyzed  in  order  to  test  the  performance  of  the  algorithm. 

Although  some  maneuver  points  were  missed,  the  maneuver  detection  algorithm 
worked  well  during  the  simulations.  The  probabilities  of  a  maneuver  being  detected  for 
noise  free  and  noisy  cases  are  shown  in  Table  1  and  Table  2.  In  order  to  obtain  these 
probabilities  a  large  number  of  simulations  {i.e.  10)  for  both  noise  free  and  noisy  cases 
were  run  on  the  IBM  PC.  Due  to  space  constraints,  just  four  representative  runs  each 
were  presented  in  the  previous  chapter..  However,  in  Table  I  and  Table  2  the  results 
of  all  ten  runs  are  shown  for  the  fixed-interval  smoothing  routine  only.  To  get  the 
probabilities  of  a  maneuver  being  detected  in  the  extended  Kalman  filter,  the  reader  must 
shift  the  numbers  in  both  tables  one  cell  right.  In  both  Table  I  and  Table  2,  the  ma¬ 
neuver  was  executed  at  N  equal  zero. 

With  a  new  maneuver  detection  technique,  the  fixed-interval  smoothing  routine  im¬ 
proved  the  accuracy  of  the  target's  position  estimates  in  all  the  simulation  cases.  This 
improvement  was  35-/5%  over  the  observed  target  positions  and  over  35-55%  over  the 
Kalman  filter's  estimates.  Applying  the  new  maneuver  detection  technique  also  im¬ 
proved  the  accuracy  of  the  extended  Kalman  filter  by  45-50%  over  the  entire  time  in- 
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Table  1.  Probabilities  of  a  Maneuver  Being  Detected  at  Point  N  in  a  Noise  Free 
Enviroinment  for  Fixed- Interval  Smoothing  Algorithm  (Maneuver  Executed 
at  N  =  0) 


El 

N=  1 

N  =  2 

Z 

It 

N  =  4 

N=  5 

0% 

0% 

25% 

75% 

100% 

100% 

Table  2.  Probabilities  of  a  Maneuver  Being  Detected  at  Point  N  in  a  Noisy 
Enviroinment  for  a  Fixed-Interval  Smoothing  Algorithm  (Maneuver  Exe¬ 
cuted  at  N  =  0) 


N  =  0 

N=  1 

N  =  2 

II 

Z 

20% 

80% 

100% 

100% 

terval.  Where  the  accuracy  was  most  improved  by  this  technique  during  the  maneuver 
periods:  here  the  accuracy  increased  by  30-60%  using  the  extended  Kalman  Filter  and 
60-80%  using  the  the  fixed-interval  smoothing  algorithm  over  the  observed  positions 
during  the  actual  maneuver  periods.  And  during  the  maneuver  periods  the  smoothed 
estimates  were  30-70%  more  accurate  than  the  Kalman  Filter's  estimates. 

These  significant  improvements  were  obtained,  in  pan,  by  using  the  time-varying 
values  of  the  state  excitation  matrix  QK.  However,  there  is  a  disadvantage  to  this  tech¬ 
nique.  Since  this  matrix  is  added  to  the  predicted  error  covariance  matrix  PK,t.u  high 
values  of  the  matrix  QK  will  cause  the  predicted  error  covariance  matrix  to  grow 
boundlessly  which  will  make  the  filter  become  unstable.  Also,  increasing  the  magnitude 
of  the  state  excitation  matrix  in  the  fixed-interval  smoothing  algorithm  makes  the 
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smoothing  filter  estimates  diverge  to  the  extended  Kalman  filter  estimates.  This  in¬ 
creases  the  need  for  a  more  accurate  extended  Kalman  filter,  since  the  accuracy  of  the 
smoothed  estimates  in  this  case  depends  to  a  large  degree  on  the  extended  Kalman  fil¬ 
ter's  estimates. 

There  are  at  least  two  areas  which  can  be  investigated  to  develop  the  tracking  algo¬ 
rithm  more  fully.  The  first  is  research  in  new  noise  models.  The  model  used  was  a  white 
noise  process.  Although  this  model  is  relatively  adequate  for  representing  atmospheric 
noise  over  an  extended  time  period,  better  models  could  be  used  which  take  into  account 
random  noise  spikes,  the  lightning  effects,  of  the  atmospheric  noise  process.  The  second 
area  is  adapting  the  algorithm  for  multi-target  tracking.  Improving  the  ability  of  the 
algorithm  to  track  and  identify  two  or  more  targets  would  have  great  value  in  ship 
tracking  and  targeting  problems. 


APPENDIX  A.  THE  EXTENDED  KALMAN  FILTER  WITH  FIXED 
INTERVAL  SMOOTHING  ALGORITHM 

C  ***  SHIPMANE. FOR  *** 

Q-kicicIt  k  A  A  A  A" A  A  A  iricMrkirrkMfiricifkititirirlr/eiflt it  A"A"A  A  A  A" A'  A  A  iV  AAcAnVArA  A  A  IticIcIrMrk A -A 'A  A  A  A  A'A'A'A* 

C*  * 

C*  THIS  A  EXTENDED  KALMAN  FILTER  TRACKING  ROUTINE  WITH  THE  FIXED  * 

C*  INTERVAL  SMOOTHING  ALGORITHM.  THIS  PROGRAM  USES  BEARINGS  TAKEN  * 

C*  FROM  TWO  SENSOR  SHIPS  TO  THE  TARGET.  A  NEW  MANEUVER  DETECTION  * 

C*  ROUTINE  IS  IMPLEMENTED.  THE  NEW  ALGORITHM  TO  DERIVE  THE  STATE  * 

C*  EXCITATION  MATRIX  Q  IS  ALSO  DEVELOPED.  TO  RUN  THE  PROGRAM:  * 

C*  * 

C*  1)  RUN  THE  PROGRAM  <RAWDATA.  FOR>  LOCATED  IN  APPENDIX  B  TO  * 

C*  PRODUCE  THE  RAW  DATA.  * 

C*  * 

C*  2)  RUN  THE  <SHIPMANE. FOR>  * 

C*  * 

C*  THE  OUTPUTS  OF  THE  PROGRAM  STORED  IN  THE  FOLLOWING  FILES:  * 

C*  * 

C*  1)  THRDATA  =  INCLUDES  THE  THERE SHOLD  VALUES  OF  THE  * 

C*  MANEUVER  GATES  AND  RESIDUAL  CALCULATED  * 

C*  FOR  EACH  OBSERVATION.  * 

C*  * 

C*  2)  CIRCDATA  =  INCLUDES  THE  REQUIRED  DATA  TO  DRAW  THE  * 

C*  MANEUVER  GATES  AS  A  CONCENTRIC  CIRCLES  * 

C*  AROUND  THE  PREDICTED  FILTER  ESTIMATES.  * 

C*  * 

C*  3)  BEGINDATA  =  INCLUDES  THE  FIRST  POINTS,  IGNORED  BY  * 

C*  THE  EXTENDED  KALMAN  FILTER,  OF  THE  TARGET  * 

C*  MANEUVERS  TO  BE  USED  BY  THE  FIXED-INTERVAL  * 

C*  SMOOTHING  ALGORITHM.  * 

C*  * 

C*  4)  MANEUDATA  =  INCLUDES  THE  DETECTED  MANEUVER  POINTS.  * 

C*  * 

C*  5)  TRUDATA  =  INCLUDES  THE  ACTUAL  POSITION  OF  THE  TARGET  * 

C*  FOR  EACH  OBSERVATION  TIME.  * 

C*  * 

C*  6)  FILDATA  =  INCLUDES  THE  EXTENDED  KALMAN  FILTER'S  POSITION  * 

C*  ESTIMATES  ALONG  WITH  THE  OBSERVED  POSITIONS,  * 

C*  KALMAN  FILTER  ERROR  AND  OBSERVATION  ERROR.  * 

C*  * 

C*  7)  SMDATA  =  INCLUDES  THE  SMOOTHED  POSITION  ESTIMATES  AND  * 

C*  SMOOTHING  ERROR.  * 

C*  * 

C*  TO  GET  THE  GRAPHIC  RESULTS:  * 

C*  * 

C*  1)  COPY  THE  FILES  TRUDATA,  FILDATA,  SMDATA  AND  MANEUDATA  INTO  * 

C*  THE  MATLAB  SUB -DIR.  * 

C*  * 

C*  2)  RUN  THE  PROGRAM  <SHIPTR.M>  IN  THE  MATLAB  SUB-DIR.  THE  * 

C*  GRAPHIC  RESULTS  WILL  BE  STORED  IN  THE  META  FILE  SHIPTR.  MET.  * 

Qirfrtrtrir/rtrtclc'trk  A  k'kir/r/r/ckirfr/rJt'itititirfrlrk  ft  A'A~/rirAk-.‘<  A  kictrlrk-/rlrir>rtr\  A  A  A  A  ***  A  A  'lViV  A'-A  A  A 
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C  ***VARIABLE  DEFINITIONS*** 


AK 

AKT 

BD 

BOC 


BRG 

BRKKM1 

DBRG 

DT 

DTOR 

FAC1 

G 

H 

HDG 

HT 

I 

IMAT 

J 

K 

PDIFF 


PHI 

PHIT 

PKK 

PKKS 

PKKM1 

PKKM1S 

IPKKM1S 

PSS 

PX 


PY 


Q 

R 

RANGE 

RTOD 

SHDG 

SPD 

SPKKM1 

SSPD 

SXPOS 

SYPOS 

TEMP 


SMOOTHING  FILTER  GAIN  MATRIX 
TRANSPOSE  OF  AK 
BAD  OBSERVATION  INDICATOR 
BAD  OBSERVATION  COUNTER  WHICH  PROVIDES 
THAT  ONLY  THE  FIRST  OF  TWO  CONCECUTIVE 
BAD  OBSERVATIONS  WILL  BE  RECOGNIZED 
MEASURED  TARGET  BEARING  IN  RADIANS 
PREDICTED  TARGET  BEARING  MEASUREMENT 
IN  RADIANS,  BRG(K/K-1) 

MEASURED  TARGET  BEARING  IN  DEGREES 
TIME  DELAY  BETWEEN  OBSERVATIONS, 

T(K)  -  T(K1) 

DEGREE  TO  RADIAN  CONVERSION  FACTOR 
RECIPROCAL  OF  VARE 
KALMAN  GAIN  VECTOR 
MEASUREMENT  MATRIX 

TARGET  HEADING  IN  DEGREES  BY  KALMAN  FILTER 

TRANSPOSE  OF  H 

COUNTER 

4X4  IDENTITY  MATRIX 
COUNTER 

ITERATION  INTERVAL 

POSITION  DIFFERENCE  BETWEEN  OBSERVED  AND 
PREDICTED  STATE  ESTIMATES 

|  Z(K)  -  XCK/K-l)  | 

DISCRETE-TIME  STATE  TRANSITION  MATRIX 
TRANSPOSE  OF  PHI 

ESTIMATION  ERROR  COVARIANCE  MATRIX,  P(K/K) 
SMOOTHED  ERROR  COVARIANCE  MATRIX 
PREDICTED  ESTIMATION  ERROR  COVARIANCE 
MATRIX,  PCK+l/K) 

PREDICTED  ERROR  COVARIANCE  MATRIX  FOR 
SMOOTHING,  PCK+l/K) 

INVERSE  OF  PKKM1S 

ERROR  COVARIANCE  MATRIX  FOR  SMOOTHING,  P(K/K) 
POSITION  DIFFERENCE  IN  X  DIRECTION  BETWEEN 
OBSERVED  AND  PREDICTED  STATE  ESTIMATES 
|  ZX  -  X( K/K-l)( 1,1)  | 

POSITION  DIFFERENCE  IN  Y  DIRECTION  BETWEEN 
OBSERVED  AND  PREDICTED  STATE  ESTIMATES 
|  ZY  -  X(K/K-1)(3, 1)  | 

STATE  EXCITATION  MATRIX 
MEASUREMENT  NOISE  COVARIANCE 
DISTANCE  FROM  SENSOR  TO  A  PRIORI  TARGET 
POSITION 

RADIAN  TO  DEGREE  CONVERSION  FACTOR 
TARGET  HEADING  IN  DEGREES  BY  SMOOTHING 
TARGET  SPEED  IN  KNOTS  BY  KALMAN  FILTER 
STORE  THE  INITIAL  ERROR  COVARIANCE,  P(0/-1) 
TARGET  SPEED  IN  KNOTS  BY  SMOOTHING 
SMOOTHED  TARGET  POSITION  IN  X  DIRECTION 
SMOOTHED  TARGET  POSITION  IN  Y  DIRECTION 
TEMPORARY  STORAGE  MATRICES  USED  IN  MATRIX 
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TIMEX 


TIMEXLO 

TL 

TLS 

TU 

TUS 

VARE 

XDIFF 

XKK 

XKKS 

XKKM1 

XKKM1S 

XPOS 

XS 

XSS 

XT 

YDIFF 

YPOS 

YS 

YT 

ZX 

ZY 


OPERATIONS 

IMPOSSIBLY  HIGH  CONSTANT  FOR  DETERMINING  THE 
FIRST  POINT  OF  THE  MANEUVER  WHICH  IS  IGNORED 
BY  THE  KALMAN  FILTER  AND  ACCOUNT  BY  THE  SMOOTHING 
FILTER 

VARIABLE  FOR  STORING  THE  MANEUVER  POINTS 
THRESHOLD  VALUE  FOR  THE  LOWER  MANEUVER  GATE 
VECTOR  VARIABLE  FOR  STORING  THE  THRESHOLD 
VALUES  OF  THE  LOWER  MANEUVER  GATE 
THRESHOLD  VALUE  FOR  TOE  UPPER  MANEUVER  GATE 
VECTOR  VARIABLE  FOR  STORING  THE  THRESHOLD 
VALUES  OF  THE  UPPER  MANEUVER  GATE 
VARIANCE  OF  RESIDUALS  PROCESS 
DISTANCE  IN  X  DIRECTION  FROM  SENSOR  TO  A 
PRIORI  TARGET  POSITION 
ESTIMATED  TARGET  STATE  VECTOR,  X(K/K) 

SMOOTHED  TARGET  STATE  VECTOR 
PREDICTED  TARGET  STATE  VECTOR,  X(K/K-1) 

PREDICTED  TARGET  STATE  VECTOR  FOR  SMOOTHING,  X(K+1/K 
KALMAN  FILTERED  TARGET  POSITION  IN  X  DIRECTION 
SENSOR  POSITION  IN  X  DIRECTION 
TARGET  STATE  VECTOR  FOR  SMOOTHING,  X(K/K) 

TRUE  TARGET  POSITION  IN  X  DIRECTION 
DISTANCE  IN  Y  DIRECTION  FROM  SENSOR  TO  A 
PTIORI  POSITION 

KALMAN  FILTERED  TARGET  POSITION  IN  DIRECTION 
SENSOR  POSITION  IN  Y  DIRECTION 
TRUE  TARGET  POSITION  IN  Y  DIRECTION 
OBSERVED  POSITION  IN  X  DIRECTION 
OBSERVED  POSITION  IN  Y  DIRECTION 


C  ***  VARIABLE  DECLARATIONS  *** 


REAL*4  XKK( 4,1), XKKM1( 4 , 1 ) , PHI ( 4 , 4) , SXPOS , SYPOS ,HDG , PDIFF 
REAL*4  H( 1,4) ,G(4, 1) ,TEMP1( 1,4) ,TEMP2( 1, 1) ,TEMP3(4, 1) ,ZT 
REAL*4  TEMP4(4,4) ,TEMP5(4,4) ,PKK(4,4) ,PKKM1(4,4) ,HT(4,1) 
REAL*4  LXKK(4, 1) ,LPKK(4,4) ,XS( 10) , YS( 10) ,DBRG( 10) ,BRG( 10) 
REAL*4  TEMP6(4,4) ,PHIT(4,4) , IMAT(4,4) ,XT, YT,SHDG,XPL( 100) 
REAL*4  VARE(2) ,E(2),G11,G13,G21,G23,Q(4,4) ,SSPD( 100) ,MC 
REAL*4  DT , XDIFF , YDIFF , RANGE , XS 1 , YS 1 , BRG1 , BRKKM1 , YPL( 100 ) 
REAL*4  OBSERR( 200 ) , FAC 1 , S IGTHT , SIGVT , R , RTOD , SPD( 100 ) , BD 
REAL*4  XS2 , YS2 , BRG2 , ZX , ZY , DTOR , TRKERR( 100 ) , TL ,TU , SX , SY 
REAL*4  XNNM 1 ( 4 , 1 ) , XSS ( 4 , 1 ) , XKKM 1 S ( 4 , 1 ) , THETA , PY , PX , PD 
REAL*4  PNNM1(4,4) ,PSS(4,4) ,PKKM1S(4,4, 100) ,IPKKM1S(4,4) 
REAL*4  AK( 4 , 4 ) , AKT( 4,4), STRKERRC 100 ) , DTS ( 1 00 ) , SPKKM 1(4,4) 

RE AL*4  TEMP 1 S ( 4 , 4 ) , TEMP2 S ( 4 , 1 ) , TEMP 3 S ( 4 , 1 ) , TH 1 ( 4 , 1 ) , YPOS 
REAL*4  TEMP4S ( 4,4), TEMP5S ( 4,4), TEMP6S ( 4 , 4 ) , TH2( 4 , 4 ) , XPOS 
REAL*4  XKKS(4, 1, 100) ,PKKS(4,4, 100) ,TLS( 100) ,TUS( 100) ,DR( 100) 
REAL*4  XPU( 100) , YPU( 100) , BOC 

INTEGER* 4  TIME, TIMEP( 100) ,NP, TIMEX, TIMEXB( 100) ,TIMEXLO( 100) 

C  ***  OPEN  OUTPUT  DATA  FILES  *** 

OPEN( UN IT=2 , F I LE= ' TRKDATA. DAT' , STATUS= ' OLD ' ) 

OPEN( UNIT=3 , FILE=' THRDATA. DAT1 ,STATUS=’NEW' ) 

OPEN( UNIT=4 , FILE= ' CIRCDATA.  DAT' , STATUS= ' NEW ' ) 
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OPEN( UNIT=6 , FILE=' MANEUDATA.  DAT’ ,STATUS='NEW' ) 

OPEN( UNIT=7 , FILE= ' TRUDATA.  DAT ' , STATUS= ' NEW ' ) 

OPEN( UNIT=8 , FILE= ' FILDATA.  DAT' ,STATUS='NEW' ) 

0PEN(UNIT=9 , FILES' SMDATA.  DAT' ,STATUS=’NEW’ ) 

C  ***  RADIAN/DEGREE  CONVERSION  FACTORS  *** 

RT0D=57.  29577951 
DT0R=0.  01745293 

C  ***  COMPUTE  4X4  IDENTITY  MATRIX  *** 

DO  5  1=1,4 
DO  5  J=l,4 
IF  (I.EQ.J)  THEN 

IMAT(I,  J)=l.  0 

ELSE 

IMAT(  I ,  J)=0.  0 

END  IF 

5  CONTINUE 

C  ***  INITIALIZE  TIME  AND  MANEUVER  DETECTION  ALGORITHM  COUNTERS  *** 

TIMEM1=0 

NP=1 

TIMEX=5000 


***  COMPUTE  BEARING  MEASUREMENT  COVARIANCE  *** 
BEARING  ERROR  STANDARD  DEVIATION  =  3  DEGREES 

R=(3*DTOR)**2 


*  THIS  WHERE  THE  EXTENDED  KALMAN  FILTERING  STARTS 


***  READ  IN  OBSERVATION  PACKET  (TIME,  #  OF  SENSORS)  *** 
DT=TIME(K)-TIME(K-1) 

WRITEC*,*)' EXTENDED  KALMAN  FILTERING  NOW  STARTS' 
WRITE(*,*)  '***  •  a  =8  -  - -irfe*' 

810  READ(2,1001 ,END=800)TIME,XT,YT,XS( 1) ,YS( 1) ,DBRG( 1) , 

*  XS(2) ,YS(2) ,DBRG(2) 

1001  FORMATC I4,8F9.  4) 

BD=0. 0 
MC=0. 0 

DC  200  L=l,2 

IF  (DBRG(L).GT.  180.0)  DBRG( L)=DBRG( L) -360 
BRG(L)=DBRG(L)*DTOR 
200  CONTINUE 

IF  (TIME.  LT.  0)  GOTO  800 
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DT=TIME-TIMEM1 
DTS(NP)=DT 

CALL  FINDPHI(PHI.DT) 

XS1=XS(1) 

YS1=YS(1) 

XS2=XS(2) 

YS2=YS(2) 

BRG1=BRG(1) 

BRG2=BRG(2) 

CALL  MP(XS1,YS1,XS2,YS2,BRG1,BRG2,ZX,ZY) 
IF(TIME.EQ.O)  THEN 

CALL  INIT(XS1,YS1,XS2,YS2,BRG1 ,BRG2,XKK,PKK) 

ENDIF 

***  PROJECT  AHEAD  STATE  ESTIMATES  *** 

X(K+1/K)  =  PHI  *  X(K/K) 

CALL  MATMUL( PHI , XKK ,4,4,1, XKKM 1 ) 

***  DERIVATION  OF  THE  Q  MATRIX  *** 

CALL  GETQC DT , XKKM 1 , PKK( 1,1),PKK(3,3),Q) 

***  PROJECT  AHEAD  ERROR  COVARIANCE  ESTIMATES  *** 

PCK+l/K)  =  (PHI  *  P(K/K)  *  PHIT)  +  Q 

CALL  MATRAN( PHI , PHIT ,4,4) 

CALL  MATMUL(PHI jPKK.AjA.A.TEMPS) 

CALL  MATMUL( TEMP6 , PHIT ,4,4,4, TEMP4 ) 

301  CALL  MATADD(TEMP4,Q,4,4,1,PKKM1) 

C  *** 

IF  (TIME.  EQ.  0)  THEN 
DO  542  1=1,4 
DO  542  J=l,4 
PKKM1( I , J)=0. 0 

542  CONTINUE 

PKKM1( 1 , 1 )=10000.  0 
PKKM1(3 ,3)=9999.  9 
PKKM1(2,2)=0.  25 
PKKM1(4,4)=PKKM1(2,2) 

DO  543  1=1,4 
DO  543  J=1 ,4 
SPKKM1( I , J)=PKKM1( I , J) 

543  CONTINUE 
ENDIF 

IF  (MC.EQ.  1.0)  GOTO  303 
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***  CALCULATE  THE  RESIDUAL  DUE  TO  THE  DIFFERENCE  BETWEEN 
OBSERVED  AND  ESTIMATED  POSITIONS  *** 

|  Z(K)  -  XCK/K-l)  |  \ 

PX=ZX-XKKM1( 1,1) 

PY=ZY-XKKM1( 3,1) 

PD=(PX**2)+(PY**2) 

PDIFF=SQRT(PD) 

***  CALCULATE  THE  MANEUVER  GATE  THRESHOLD  VALUES  *** 

CALL  MANDET(TIME, PDIFF, XKKM1( 1,1) ,XKKM1(3, 1) ,PKKM1( 1 , 1) , 

*  PKKM1(3,3) ,PKKM1(1,3) ,XPL,YPL,XPU, YPU,TL,TU) 

DO  640  IE=1,37 

WRITE(4,*)XPL( IE) ,YPL( IE) ,XPU( IE) ,YPU( IE) 

>0  CONTINUE 

***  STORE  THE  MANEUVER  GATE  VALUES  AND  RESIDUAL  DUE  TO  THE 
POSITION  DIFFERENCE  *** 

TLS(NP)=TL 
TUS(NP)=TU 
DR(NP)=PDIFF 

C  ***  MANEUVER  DETECTION/DIVERGENCE  ALGORITHM  *** 

IF  ( ( PDIFF.  GE.  TL) .  AND.  ( PDIFF.  LE.  TU) )  THEN 
WRITEC*,*)' MANEUVER  POSSIBILITY' 

MC=1.  0 
ZT=ZT+1 

IF  (ZT.GE.  2.0)  THEN 

IF  (TIMEX.  GT.  TIME)  THEN 
TIMEX=TIME 
TIMEXB(NP)=TIME 

TIMEXLO(NP)=TIMEXB(NP) -( 1*DTS(NP)) 

WRITEC 5, 1042)TIMEXB(NP) ,TIMEXLO(NP) 

F0RMAT(2I4) 

ENDIF 

CALL  MATSCL(2. 0,Q,4,4,Q) 

TIMEP(NP)=TIME 
WRITEC 6, 1048 )TIMEP(NP) 

FORMATC 14) 

GOTO  301 
ENDIF 
ELSE 

ZT=0. 0 
TIMEX=5000 
ENDIF 

C  ***  RECOGNAZITION  OF  THE  BAD  OBSERVATIONS  *** 

IF  ( PDIFF.  GE.TU)  THEN 
IF  (BOC.NE.  1.  0)THEN 
BD=1.  0 
B0C=1.  0 


1042 


1048 
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ELSE 

B0C=0.  0 
END  IF 
ELSE 

B0C=0. 0 
ENDIF 


303  MC=0. 0 

C  *** 

IF  (TIME.  EQ.  0.  0)  THEN 
DO  544  1=1,4 
DO  544  J=1 ,4 
PKKM1( I, J)=SPKKM1( I , J) 
544  CONTINUE 

ENDIF 


204  CONTINUE 

DO  210  L=l,2 

C  ***  CALCULATE  RANGE  TO  TARGET  *** 

XDIFF=XKKM1( 1 , 1 ) -XS( L) 

YDIFF=XKKM1( 3 , 1) -YS(L) 
RANGE=SQRT(XDIFF**2+YDIFF**2) 

***  UPDATE  H  MATRIX  WITH  LATEST  STATE  ESTIMATES  *** 
AND  CALCULATE  MEASUREMENT  ERROR  *** 

H( 1 , 1)=YDIFF/RANGE**2 
H(l,2)=0.0 

H( 1 , 3 )=-XDIFF/RANGE**2 
H(l,4)=0.  0 

BRKKM1=ATAN2( XDIFF , YDIFF) 

E( L)=BRG( L) -BRKKM1 

***  COMPUTE  KALMAN  GAIN  MATRIX  *** 
G=PKKM1*HT*(H*PKKM1*HT+R)**( -1) 

CALL  MATRAN(H,HT, 1,4) 

CALL  MATMUL(H,PKKM1 , 1,4, 4, TEMPI) 

CALL  MATMULC TEMP 1,HT, 1,4, 1,TEMP2) 
VARE(L)=TEMP2( 1, 1)+R 
FAC1=1/VARE(L) 

CALL  MATMUL(PKKM1,HT,4,4, 1.TEMP3) 

CALL  MATSCL(FAC1,TEMP3,4, 1,G) 

C  ***  COMPANSATION  OF  THE  BAD  OBSERVATIONS  *** 

IF  (BD.EQ.  1.0)  THEN 
DO  730  1=1,4 
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on  o  on  on 


G(I,1)=0. 0 
730  CONTINUE 

ENDIF 

C  *** 

IF  (L.EQ.  1)  THEN 
G11=G( 1 , 1) 

G13=G(3,1) 

ELSE 

G21=G( 1, 1) 

G23=G(3,1) 

ENDIF 

***  COMPUTE  UPDATED  ESTIMATE  *** 

X(K/K)  =  XCK/K-l)  +  G  *  E,  WHERE  E  =  Z(K)  -  H(K)*X(K/K-1) 

XKK( 1, 1)=XKKM1( 1, 1)+(G( 1, 1)*E(L)) 

XKK(  2 , 1)=XKKM1(  2 ,  l)+(  G(  2 , 1)*E(  L)  ) 

XKK( 3 , 1)=XKKM1( 3 , 1)+(G( 3 , 1)*E(L) ) 

XKK(4, 1)=XKKM1(4, 1)+(G(4, 1)*E(L)) 

***  COMPUTE  UPDATED  ERROR  COVARIANCE  MATRIX  *** 

P(K/K)  =  (I  -  G*H)  *  P(K/K-1) 

CALL  MATMUL(G,H,4, 1 ,4,TEMP4) 

CALL  MATSUB( IMAT,TEMP4,4,4,TEMP5) 

CALL  MATMUL( TEMP5 , PKKM 1 , 4 , 4 , 4 , PKK) 

***  IF  MORE  MEASUREMENTS,*** 

IF  (L.LT.  2)  THEN 

***  USE  UPDATED  STATE  AND  ERROR  COVARIANCE  ESTIMATES  FOR  NEXT 
MEASUREMENT  *** 

DO  150  1=1,4 
DO  150  J=l,4 

PKKM1( I , J)=PKK( I , J) 

XKKM1( I , 1)=XKK( 1,1) 

150  CONTINUE 

ENDIF 

210  CONTINUE 


C  ***  THESE  STATEMENTS  ARE  FOR  THE  SMOOTHING  ALGORITHM  *** 

DO  620  1=1,4 
XKKS( I , 1,NP)=XKK( 1,1) 

620  CONTINUE 


DO  630  1=1,4 
DO  630  J=1 ,4 

PKKS(I,J,NP)=PKK(I,J) 
630  CONTINUE 
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C  ***  COMPUTE  TRUE  TRACKING  AND  OBSERVATION  ERRORS  *** 


TRKERR(NP)=SQRT( (XT-XKKC 1 , 1) )**2+( YT-XKK( 3 , 1) )**2) 
OBSERR(NP)=SQRT((XT-ZX)**2+(YT-ZY)**2) 

C  ***  COMPUTE  ESTIMATED  X-Y  POSITION,  COURSE,  AND  SPEED  *** 

XPOS=XKK( 1,1) 

YPOS=XKK( 3,1) 

IF  (XKK( 2,1).  EQ.  0  .AND.  XKK(4, 1).  EQ.  0)  THEN 
HDG=0. 0 

ELSE 

HDG=RT0D*ATAN2(XKK(2,1),XKK(4,1)) 

ENDIF 

IF  (HDG.  LT.  0.0)  HDG=HDG+360 

SPD( NP)=60*SQRT( XKK( 2 , 1 )**2+XKK( 4 , 1 )**2) 

WRITE ( 7 , 10 1 1 ) TIME , XT , YT 

1011  FORMAT( 14, 2F15.  4) 

WRITEC  8 , 1012)TIME ,NP , XPOS , YPOS , ZX , ZY , TRKERR( NP) , OBSERR( NP) , 
*  PKK( 1,1) 

1012  F0RMAT(2I4,7F15. 4) 

C  ***  UPDATE  DATA  COUNTER  *** 

NP=NP+1 

TIMEM1=TIME 

GOTO  810 


800  NP=NP-1 

C*******-k*****k**ttirk**1rk1rkick****+1t***]'*******irk******irk***-l'**-k/'+**-k** 

C*  THIS  IS  WHERE  THE  FIXED-INTERVAL  SMOOTHING  ALGORITHM  STARTS  * 

A  A'  iV"A  fricicirirlririricirkic/t+rfc'fcMcIck-icicIrlrfcleMricMrlciricJc 

WRITE (*,*)' FIXED -INTERVAL  SMOOTHING  NOW  STARTS' 

WRITE(*,*)' 

DO  3000  KK=1,NP-1 

K=NP-KK 

DT=DTS(K+1) 

TIME=TIMEM1 -DT 
CALL  FINDPHI(PHI.DT) 

DO  901  1=1,4 
XSS( I , 1)=XKKS( I, 1,K) 

901  CONTINUE 

DO  902  1=1,4 
DO  902  J=l,4 
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PSS( I , J)=PKKS(I , J,K) 

902  CONTINUE 

C  ***  CALCULATE  THE  PREDICTED  STATE  ESTIMATES  *** 

C  X( K+1/K)=PHI*X( K/K) 

CALL  MATMUL  (PHI,XSS,4,4,1,XKKM1S) 

C  ***  DERIVATION  OF  THE  Q  MATRIX  *** 

CALL  GETQ(DT,XKKM1S,PSS( 1, 1) ,PSS(3,3) ,Q) 

C  ***  CALCULATION  OF  THE  PREDICTED  ERROR  COVARIANCE  MATRIX 
C  AND  COMPANSATION  ALGORITHM  WHICH  USES  THE  MANEUVER 

C  PERIOD  DETECTED  IN  THE  EXTENDED  KALMAN  FILTER  ROUTINE  *** 
C  P(K+1/K)=PHI*P(K/K)*PHIT+Q 

CALL  MATRAN  (PHI ,PHIT,4,4) 

CALL  MATMUL( PHI ,  PSS ,  4 , 4 , 4 , TEMP6 ) 

CALL  MATMUL( TEMP6 , PHIT , 4 , 4 , 4 , TEMP4 ) 

IF  (TIME. EQ. TIMEP(K) )  THEN 

IF  (TIME. EQ.  TIMEP( 1) )  GOTO  483 
CALL  MATSCL(2.0,Q,4,4,Q) 

ELSE 

305  READ(6, 1051,END=482)TIMEM,TIMEL 

1051  F0RMAT(2I4) 

IF  (TIME.  EQ.  TIMED  THEN 
CALL  MATSCL(2.  0,Q,4,4,Q) 

END  IF 
GOTO  305 
ENDIF 

482  REWIND  6 

483  CALL  MATADD(TEMP4,Q,4,4, 1,PKKM1S) 

C  ***  CALCULATE  THE  SMOOTHING  FILTER  GAIN  MATRIX  *** 

C  AK=P(K/K)*PHIT*INV#P(K+1/K) 

CALL  MATINV  (PKKM1S,4,IPKKM1S) 

CALL  MATMUL  (PHIT,IPKKM1S,4,4,4,TEMP1S) 

CALL  MATMUL  (PSS,TEMP1S,4,4,4,AK) 


DO  904  1=1,4 

XNNM1( I , 1 )=XKKS( I , 1 , K+l ) 
904  CONTINUE 


C  ***  CALCULATE  THE  SMOOTHED  STATE  ESTIMATE  *** 
C  XKKS=X( K/K)+AK*( X( K+l/N) -X( K+l/K) 

CALL  MATSUB  (XNNM1 .XKKM1S ,4, 1 .TEMP2S) 
CALL  MATMUL  (AK,TEMP2S,4,4, 1.TEMP3S) 
CALL  MATADD  (XSS,TEMP3S,4, 1, 1,TH1) 

DO  903  1=1,4 
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n  o 


XKKS( I , 1 ,K)=TH1( 1,1) 

903  CONTINUE 

DO  906  1=1,4 
DO  906  J=1 ,4 

PNNH1( I , J)=PKKS( I , J,K+1) 
906  CONTINUE 


***  CALCULATE  THE  SMOOTHED  COVARIANCE  MATRIX  *** 
PKKS=P(K/K)+AK*|P(K+1/N)-P(K+1/K)1*AKT 

CALL  MATSUB  (PNNM1,PKKM1S,4,4,TEMP4S) 

CALL  MATRAN  (AK,AKT,4,4) 

CALL  MATMUL  (AK.TEMP4S ,4,4,4, TEMP5S) 

CALL  MATMUL  (TEMP5S ,AKT, 4,4,4, TEMP6S) 

CALL  MATADD  (PSS,TEMP6S,4,4, 1,TH2) 

DO  908  1=1,4 
DO  908  J=1 ,4 
PKKS( I , J,K)=TH2( I , J) 

908  CONTINUE 


C  ***  COMPUTE  ESTIMATED  X-Y  POSITION,  COURSE,  AND  SPEED  *** 

IF  (XKKS(2,1,K). EQ. 0  .AND.  XKKS(4,1,K). EQ.  0)  THEN 
SHDG=0. 0 

ELSE 

SHDG=RTOD*ATAN2(XKKS( 2 , 1 ,K) , XKKS(4 , 1 ,K) ) 

ENDIF 

IF  (SHDG.LT.O.O)  SHDG=SHDG+360 
SSPD(K)=60*SQRT(XKKS(2,.T,K)**2+XKKS(4,1,K)**2) 

TIMEM1=TIME 

3000  CONTINUE 

REWIND  4 

C  ***  CALCULATE  THE  SMOOTHED  TRACKING  ERROR  *** 

DO  1100  K=1,NP 

SXPOS=XKKS ( 1 , 1 , K ) 

SYPOS=XKKS( 3 , 1 ,K) 

READ( 4 , 1 1 10 )TIME , XT ,  YT 

STRKERR(K)=SQRT((XT-XKKS(1,1,K))**2+(YT-XKKS(3,1,K))**2) 

WRITE ( 9 , 1120)K,SXP0S , SYPOS , STRKERR(K) , PKKS( 1 , 1 ,K) 

1100  CONTINUE 

1110  FORMATC I4.2F15.  4) 

1120  FORMATC 14, 4F20.  4) 

CL0SE(UNIT=2) 

CL0SE(UNIT=3) 
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CL0SE(UNIT=4) 

CL0SE(UNIT=5) 

CL0SE(UNIT=6) 

CL0SE(UNIT=7) 

CLOSE ( UN IT=8) 

CLOSE(UNIT=9) 

WRITE(*,*)' THERE  WERE',NP,'  OBSERVATIONS  PROCESSED.’ 
WRITE(*,*)’FOR  GRAPHIC  RESULTS  COPY' 

WRITEC*,*)’  1)  FILDATA.DAT’ 

WRITE(*,*)’  2)  MANEUDATA.DAT’ 

WRITE(*,*)’  3)  SMDATA.DAT' 

WRITE(*,*)'  4)  TRUDATA.DAT’ 

WRITE(*,*)’TO  THE  MATLAB  SUB-DIRECTORY  AND  RUN  =>  <SHIPTR.M>’ 

STOP 

END 


SUBROUTINE  FINDPHI ( PHI , DT) 

C  COMPUTES  THE  VALUES  OF  THE  PHI  MATRIX 

Q  -k-Airlcft  A-Mi  A rcUnfc  A  A  ft  /rk-irirk icirk it  it hh  ir/tiHtlrlticirlr/tir/rk-fcin 

REAL*4  PHI(4,4) ,DT 

DO  1501  1=1,4 
DO  1501  J=l,4 
DO  1501  K=l,2 

PHI( I , J)=0.  0 

1501  CONTINUE 

C  ***  COMPUTE  PHI  MATRIX  *** 

DO  1500  1=1,4 
PHI(I,I)=1.  0 
1500  CONTINUE 

PHI( 1,2)=DT 
PHI( 3 ,4)=DT 

RETURN 

END 


SUBROUTINE  INIT(XSl,YSl,XS2,YS2,BRGl,BRG2,?aCK,PKK) 

C  THIS  ROUTINE  INITIALIZES  THE  STATE 

C  AND  ERROR  COVARIANCE  ESTIMATES 

REAL*4  XKK(4, 1) ,PKK(4,4) 

REAL*4  XS1,YS1,XS2,YS2,BRG1,BRG2 
REAL*4  NUMER.DENOM 
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C  ***  INITIAL  STATE  ESTIMATE  *** 

NUMER=( -YS2*TAN(BRG2))+(YS1*TAN(BRG1))+XS2-XS1 
DENOM=TAN( BRG1) -TAN( BRG2) 

XKK( 3 , 1)=NUMER/DEN0M 
XKK( 2, 1)=0.  0 

XKK(1,1)«(XKK(3,1)-YS1)*TAN(BRG1)+XS1 
XKK(4,1)=0.  0 

C  ***  INITIAL  ERROR  COVARIANCE  ESTIMATE  *** 

DO  555  1=1,4 
DO  555  J=1 ,4 
PKK(I,J)=0. 0 
555  CONTINUE 

PKK( 1, 1)=10000.  0 
PKK(3,3)=9999.  9 
PKK(2,2)=0.  25 
PKK(4,4)=PKK(2,2) 

RETURN 

END 


SUBROUTINE  GETQ(DT,XKKM1L,P11,P33,Q) 

(]**itit*A*JrWA<!<nlr***)HHritfrlr*<r*il(A**iHl*****ilr*AA'jntilr*********j|i***A**'ilr*'<r*)it;(;^* 

C  CALCULATES  STATE  EXCITATION  MATRIX  Q 

QMr/r/rkicfi-lrie-irkrlricir/e-Mth‘krlr/iAir/eirir)rk-itir/rkiririrk-/cfrkit  Mrk  iVA/r  A 

REAL*4  DT,XKKM1L(4, 1) ,QT,P11,P33,NT,Q(4,4) 

REAL*4  QM,VT,SIGTHT,SIGVT 

SIGTHT=0.  0001 
SIGVT=0.  0001 

IF  ((XKKM1LC 2,1).  EQ.  0).  OR.  (XKKM1L(4, 1).  EQ.  0) )  THEN 
QT=0. 0 
GOTO  200 
END  IF 

VT=SQRT( (XKKM1L( 2 , 1)**2)+(XKKM1L( 4, 1)**2) ) 

IF  (Pll.  GT.  P33)  THEN 

QT=(  (  (XKKM1LC  2 ,  l)/VT)**2)*SIGVT)+(  (XKKM1L(4,  l)**2)*SIGriTT) 
ELSE 

QT=(((XKKM1L(4,1)/VT)**2)*SIGVT)+((XKKM1L(2,1)**2)*SIGTHT) 

ENDIF 

200  NT=(DT**4)/4. 0 

QM=NT*QT 

DO  556  1=1,4 
DO  556  J=1 ,4 
Q(  I ,  J)=0.  0 
556  CONTINUE 
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onno  oooo 


557 


DO  557  1=1,4 
Q(I,I)=QM 
CONTINUE 

CALL  MATSCL(0.  1,Q,4,4,Q) 

RETURN 

END 


SUBROUTINE  MP( XS 1 , YS 1 , XS  2 , YS  2 , BRG 1 , BRG2 , ZX , Z Y ) 

THIS  ROUTINE  COMPUTES  THE  OBSERVED  X,  Y  POSITIONS  OF  THE 
TARGET  USING  SENSOR  SHIP  POSITIONS  AND  BEARINGS  TO  THE  TARGET 

tirfc&Irk-te-iririeiririririfklriciciricir/c'icfcitlrirk-ltlriciciciriFlr/riticIciclirk  A/ra’cA/tA  Irk  AAA  'AA  AAA  AA  '/mV  A  -A  A  A 

REAL*4  ZX,ZY 

REAL*4  XS1,YS1,XS2,YS2,BRG1,BRG2 
REAL*4  NUMER.DENOM 

NUMER=C -YS2*TAN( BRG2 ) )+( YS 1*TAN( BRG1 ) )+XS2 -XS 1 
DENOM=TAN( BRG1) -TAN( BRG2 ) 

Z Y=NUMER / DENOM 
ZX=( ZY-YS1)*TAN( BRG1)+XS1 

RETURN 

END 

SUBROUTINE  MANDET(TIME,DIFF,XT,YT,P1,P3,P13,XPL,YPL, 

*  XPU,YPU,TL,TU) 

AAAAAAAAAAA-AAA  AAr/rAA  ft  AtAAAA-AAtAAA  AA  AAAr  AA  A  AAAArAArAArAArA-A  A  AAAA  AAA  A 

THIS  SUBROUTINE  COMPUTES  THE  THRESHOLD  VALUES  OF  THE 
MANEUVER  GATES  USING  ERROR  ELLIPSE  EQUATIONS 

A*AAAAAAftft-.,cA*A-AYrft'AAft*A*Aft-ftAAYtAft,ftft''..WrftAftAAlftAftAAftArt  A'AftArAAAftft'ftftftftAAAA'A  A'* 

REAL*4  XT,YT,XPL(21) ,YPL( 21) ,XPU(21) ,YPU(21) ,THE1,SIG2X 
REAL*4  SX,SY,CT,P1,P13 ,P3,DIFF,TL,TU,C,D,DTOR,A,B,SIG2Y 
REAL*4  THETA, DIV 

INTEGERS  NP, TIME, CO 

DT0R=0.  0174529 
DIV=30.  0 
A=2*P13 
B=P1-P3 

THE1=0. 5*ATAN2(A,B) 

C=(Pl+P3)/2 
D=0.  0 

IF  (P13.EQ.  0.  0)  GOTO  10 
D=P13/SIN(2.  0*THE1) 

10  SIG2X=ABS(C+D) 

SIG2Y=ABS(C-D) 

SX=(SIG2X**0.  5) 

SY=( SIG2Y**0. 5) 

IF  (SX.GT. SY)  THEN 
TL=3*SX 
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TU=8*SX 

ELSE 

TL=3*SY 

TU=8*SY 

ENDIF 

CT=C0S(THE1) 

ST=SIN(THE1) 

IF  (TIME.  GT.  0)  THEN 
TL=TL/DIV 
TU=TU/DIV 
ENDIF 

WRITE(3,1045)TIME,TL,TU,DIFF 
1045  FORMAT(I4,3F10. 4) 

DO  100  IE=1,37 
CO=IE-l 

THETA=( (360/36) *CO)*DTOR 
XPU( IE )=TU*COS( THETA )+XT 
YPU( IE )=TU*SIN( THETA )+YT 

100  CONTINUE 

DO  120  IE=1,37 
CO=IE-l 

THETA=( ( 360/36)*C0)*DT0R 
XPL( IE )=TL*COS( THETA )+XT 
YPL( IE)=TL*SIN(THETA)+YT 

120  CONTINUE 

RETURN 

END 


SUBROUTINE  MATMUL(A,B,L,M,N,C) 


C  THIS  ROUTINE  MULTIPLIES  TWO  MATRICES  TOGETHER 

REAL*4  A(L,M) ,B(M,N) ,C(L,N) 

DO  10  1=1, L 
DO  10  J=1,N 
C(I,J)=0.0 
10  CONTINUE 

DO  100  1=  1,L 
DO  100  J=  1,N 
DO  100  K=  1,M 

C(I,J)  =  C(I,J)  +  A( I ,K)*B(K, J) 

100  CONTINUE 

RETURN 

END 
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SUBROUTINE  MATRAN(A,B,N,M) 


THIS  ROUTINE  TRANSPOSES  A  MATRIX 
0  B(M,N)  =  A'(N,M) 


REAL*4  A(N,M),  B(M,N) 

DO  100  1=  1,N 
DO  100  J=  1 ,M 
B(J,I)  =  A( I , J) 

100  CONTINUE 

RETURN 

END 


SUBROUTINE  MATSCL(SC,A,N,M,C) 


THIS  ROUTINE  MULTIPLIES  A  MATRIX  WITH  A  SCALAR 
0  C(N,M)  =  SC  *  A(N,M) 


REAL*4  A(N,M),  C(N,M),  SC 

DO  100  I  *  1,N 
DO  100  J  =  1 ,M 
C(I,J)  =  SC*A( I , J) 

100  CONTINUE 

RETURN 

END 


SUBROUTINE  MATSUB(A,B ,N,M,C) 


THIS  ROUTINE  SUBTRACTS  TWO  MATRICES 
#  C(N,M)  =  A(N,M)  -  B(N,M) 


REAL*4  A(N,M),B(N,M),C(N,M) 

DO  100  I  =  1,N 
DO  100  J  =  1,M 
C( I , J)=A( I , J) -B( I ,  J) 

100  CONTINUE 

RETURN 

END 

SUBROUTINE  MATADD(A,B>N>M,L,C) 


THIS  ROUTINE  ADDS  TWO  MATRICES 
0  C(N,M)  =  A(N,M)  +  B(N,M) 
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REAL*4  A(N,M),B(N,M),C(N,M,L) 
DO  100  I  =  l,N 
DO  100  J  =  1,M 
C(I,J,L)=A(I, J)+B(I,J) 

100  CONTINUE 

RETURN 

END 


SUBROUTINE  MATINV  (A,N,C) 


C  THIS  ROUTINE  COMPUTES  THE  INVERSE  OF 

C  A  MATRIX 

C  C(N,N)=INV  | A(N,N) | 


REAL*4  A(N,N) ,C(N,N) ,D(20,20) 
DO  100  I  =  1,N 
DO  100  J  =  1,N 
100  D(I, J)=A(I,J) 


DO  115  1=1, N 
DO  115  J=N+1,2*N 
115  D( I , J)=0. 0 


DO  120  1=1, N 
J=I+N 

120  D(I , J)=l. 0 

DO  240  K=1,N 
M=K+1 

IF  (K.EQ.N)  GOTO  180 
L=K 

DO  140  I=M,N 

140  IF  (ABS(D(I,K)).GT.  ABS(D(L,K)))  L=I 

IF  (L.EQ.K)  GOTO  180 

DO  160  J=K,2*N 
TEMP=D(K, J) 

D(K, J)=D(L, J) 

160  D(L,J)=^rEMP 


180  DO  185  J=M,2*N 

185  D(K, J)=D(K, J)/D(K,K) 

IF  (K.EQ.  1)  GOTO  220 
M1=K-1 

DO  200  1=1, Ml 
DO  200  J=M , 2*N 

200  D(I,J)=D(I,J)-D(I,K)*D(K,J) 

IF  (K.EQ.N)  GOTO  260 

220  DO  240  I=M,N 

DO  240  J=M , 2*N 

240  D(I,J)=D(I,J)-D(I,K)*D(K,J) 
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260 

DO  265  1=1, N 

DO  265  J=1,N 

K=J+N 

265 

G(I,J)=D(I,K) 

RETURN 

END 
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APPENDIX  B.  INPUT  DATA  FILE  FORMATTING  ALGORITHM 

C  ***  RAWDATA.  FOR  *** 


C* 


* 


C*  THIS  PROGRAM  EMPLOYES  A  TARGET  AND  TWO  SENSOR  SHIPS.  IT  ASKS  FOR  * 
C*  THE  INITIAL  POSITIONS,  SPEEDS  AND  COURSES  OF  THE  TARGET  AND  SENSOR  * 
C*  SHIPS.  IT  ALSO  CALLS  FOR  MANEUVER  PERIOD,  THE  SPEED  AND  THE  COURSE  * 
C*  CHANGE  OF  THE  TARGET  DURING  THIS  MANEUVER  PERIOD.  THE  OUTPUTS  * 
C*  WHICH  CONSIST  OF  NOISY  OR  NOISE  FREE  BEARINGS  FROM  THE  SENSOR  SHIPS  * 
C*  TO  THE  TARGET  AND  POSITIONS  OF  THE  EACH  SHIPS  ARE  STORED  IN  THE  FILE* 
C*  TRKDATA.  DAT  TO  BE  USED  BY  THE  PROGRAM  <SHIPMANE.  FOR>.  * 


C  ***  VARIABLE  DEFLATIONS  *** 


C  BRG 

C  CASE 

C 

C  CS ,  CE 

C  DTOR 

C  END 

C  HDGS 

C  HDGT 

C  HDGTD 

C  MS,  ME 

C  Nl,  N2 

C  NM 

C  PER 

C  RTOD 

C  SS,  SE 

C  SPDS 

C  SPDT 

C  SPDTD 

C  TIMED 

C  UNHDGCH 

C  UNSPDCH 

C  XDIFF,  YDIFF 

C 

C  XS 

C  XT 


MEASURED  TARGET  BEARINGS. 

INDICATOR  OF  THE  NOISE  EXISTANCE.  THE  NOISE  EXISTS 
FOR  POSITIVE  VALUE,  NO  NOISE  FOR  NEGATIVE  VALUE. 
START  AND  END  HEADINGS  OF  THE  MANEUVER. 

DEGREE  TO  RADIAN  CONVERSITION  FACTOR. 

END  OF  THE  TRACKING  PROBLEM. 

SENSOR  SHIP’S  HEADING. 

TARGET'S  HEADING. 

TOTAL  HEADING  CHANGE  DURING  THE  MANEUVER. 

START  AND  END  TIMES  OF  THE  MANEUVER. 

MEASUREMENT  NOISESS. 

NUMBER  OF  MANEUVERS. 

OBSERVATION  PERIOD. 

RADIAN  TO  DEGREE  CONVERSITION  FACTOR. 

START  AND  END  TIMES  OF  THE  MANEUVER. 

SENSOR  SHIP'S  SPEED. 

TARGET'S  SPEED. 

TOTAL  SPEED  CHANGE  DURING  THE  MANEUVER. 

TOTAL  MANEUVER  TIME. 

HEADING  CHANGE  PER  OBSERVATION. 

SPEED  CHANCE  PER  OBSERVATION. 

THE  DISTANCES  IN  THE  X  AND  Y  DIRECTIONS  FROM  SENSOR 
TO  A  TARGET  POSITION. 

SENSOR  SHIP'S  STATES. 

TARGET'S  STATES. 


C  ***  VARIABLE  DECLE RATIONS  *** 

REAL*4  XT(4, 1) ,XS1(4, 1) ,PHI(4,4) , SPDS 1, HDGS 1,SPDS2,HDGS2,SP, HD 
REAL*4  DT , SPDT , HDGT , XS2 ( 4 , 1 ) , TEMP 1 ( 4 , 1 ) , CASE , XDIFF 1 , YD IFF 1 
REAL*4  XDIFF2,YDIFF2,N1,N2, DTOR, RTOD, BRG1,BRG2,CS,CE( 20), HDGTD 
REAL*4  MS(20) ,ME(20) ,SS,SE(20) , SPDTD, UNSPDCH( 10) , UNHDGCH( 10) 


INTEGER  TIME, TIMEMl.NM, PER, END 
C  ***  OPEN  DATA  FILES  *** 
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OPENC UNIT=2 , FILE= ' NOISE 1 .  DAT ' , STATUS= ’ OLD ' ) 
OPEN(UNIT=3,FILE=' NOISE2.DAT' , STATUS= ' OLD ' ) 
OPEN( UNIT=4 , F I LE= ' TRKDATA.  DAT f , STATUS= ’ NEW ' ) 


WRITEC*,*) 'ENTER  A  NEGATIVE  NUMBER  FOR  NOISELESS  CASE; ' 
WRITEC*,*) 'POSITIVE  FOR  NOISY  CASE' 

READC*,*) CASE 

TIMEM1=0 

RTOD=57. 29577951 
DT0R=0.  017453293 

WRITEC* ,*)' ENTER  THE  OBSERVATION  PERIOD  AND' 

WRITEC*,*)' END  OF  THE  OBSERVATION  TIME. ’ 

RE ADC*,*) PER, END 

WRITEC*,*)’ INPUT  DESIRED  INITIAL  X  POSITION,  Y  POSITION,’ 
WRITEC*,*)’ SPEED  CIN  KNOTS)  AND  COURSE  CIN  DEGREES)  OF  TARGET’ 
READC * , * ) XTC 1 , 1 ) , XTC 3 , 1 ) , SPDT , HDGT 

SP=SPDT 

HD=HDGT 

XTC 2 , 1 )=C SPDT/60 )*SINC HDGT*DTOR) 

XTC 4 , 1 )=C SPDT/ 60 )*COSC HDGT*DTOR) 

WRITEC*,*) ’FOR  SENSOR  Is ’ 

WRITEC*,*) ’INPUT  DESIRED  INITIAL  X  POSITION,  Y  POSITION,’ 
WRITEC*,*) ’SPEED  CIN  KNOTS)  AND  COURSE  CIN  DEGREES)’ 
READC*,*)XS1C1,1),XS1C3,1),SPDS1,HDGS1 

XS1C2,1)=CSPDS1/60)*SINCHDGS1*DT0R) 

XS1C4,1)=CSPDS1/60)*COSCHDGS1*DTOR) 

WRITEC*,*) ’FOR  SENSOR  2:  ’ 

WRITEC*,*) ’INPUT  DESIRED  INITIAL  X  POSITION,  Y  POSITION,’ 
WRITEC*,*) ’SPEED  CIN  KNOTS)  AND  COURSE  CIN  DEGREES)’ 

READC*, *)XS2C 1 , 1) ,XS2( 3, 1) ,SPDS2 ,HDGS2 

XS2C  2, 1)=C SPDS2/60)*SINCHDGS2*DTOR) 

XS2C  4 , 1)=C  SPDS2/60)*C0SC  HDGS2*DT0R) 

WRITEC*,*) ’HOW  MANY  TIMES  DO  YOU  WANT  TO  MAKE  MANEUVER?’ 
READC*,*)NM 

DO  540  K=1.NM 
WRITEC*,*)  ’ 

WRITEC*,*) '**MANEUVER  #’ ,K 

510  WRITEC*,*) ’ENTER  THE  STARTING  AND  ENDING  TIMES  OF’ 

WRITEC*,*) ’THE  MANEUVER  #' ,K 
READC*,*)MSCK),MECK) 

IF  CCMSCK).  GT.  END). OR.  CMECK).  GT.  END))  THEN 
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WRITEC*,*) 'CAREFULL!  END  OF  THE  TRACKING  IS' , END 
WRITEC*,*)'  ' 

GOTO  510 
END  IF 

TIMED=ME(K)-MS(K) 

520  WRITEC* ,*)' ENTER  THE  STARTING  AND  ENDING  SPEEDS  OF* 

WRITEC*,*)' THE  SPEED  MANEUVER  #' ,K 
READC*,*)SS,SECK) 

IF  CSS.NE.SP)  THEN 

WRITEC*,*) 'CAREFULL!  CURRENT  SPEED  IS',SP 
WRITEC*,*)'  ' 

GOTO  520 
END  IF 

SP=SECK) 

SPDTD=SECK)-SS 

UNSPDCHCK)=C  SPDTD/TIMED)*PER 

530  WRITEC*,*) 'ENTER  THE  STARTING  AND  ENDING  COURSES  OF* 

WRITEC*,*) 'THE  COURSE  MANEUVER  #' ,K 
READC*,*)CS,CE(K) 

IF  CCS.  NE.  HD)  THEN 

WRITEC*,*)' CAREFUL!  CURRENT  HEADING  IS' , HD 
WRITEC*,*)'  * 

GOTO  530 
ENDIF 

HD=CECK) 

HDGTD=CECK)-CS 

UNHDGCHC  K ) =C  HDGTD /TIMED ) *PER 

540  CONTINUE 

DO  610  J=l,  1000 
DO  550  L=1,NM 

IF  CCTIME.GT. CMS(L))).AND.  CTIME.LE.  CMECL))))  THEN 

SPDT=SPDT+UNSPDCH( L) 

HDGT=HDGT+UNHDGCHC  L) 

IF  CCUNSPDCHCL).  LT.  0.  0).  AND.  CSPDT.  LT.  SECL)))  SPDT^SECL) 
IF  C CUNSPDCHC L). GT. 0.  0 ).  AND.  CSPDT.  GT.  SECL)))  SPDT=SECL) 

IF  C C UNHDGCHC L).  LT.  0.  0 ).  AND.  CHDGT.LT.  CECL)))  HDGT=CECL) 
IF  CC UNHDGCHC L).GT.  0.0).  AND.  CHDGT.GT.  CECL)))  HDGT=CECL) 

XTC  2 , 1)=C  SPOT/ 60 )*S INC  HDGT*DTOR) 

XTC4,  l)=(SPDT/60)*C0SCHDGTr,DT0R) 

ENDIF 
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S50  CONTINUE 

C  **★  UPDATE  TARGET  AND  SENSOR  STATES  TO  MEASUREMENT  TIME  *** 
DT=TIME-TIMEM1 


C  ***  COMPUTE  PHI  MATRIX  *** 

CALL  FINDPHI(DT,PHI) 

C  ***  UPDATE  TARGET  STATES  *** 

CALL  MATMUL( PHI, XT, 4 ,4,1, TEMPI) 
DO  560  1=1,4 

XT( I , 1)=TEMP1(I, 1) 

560  CONTINUE 

q  ***  UPDATE  SENSOR  STATES  *** 

CALL  MATMUL(PHI,XS1, 4,4,1, TEMPI) 
DO  570  1=1,4 

XS1(I,1)=TEMP1(I,1) 

570  CONTINUE 

CALL  MATMUL(PHI,XS2,4,4, 1,TEMP1) 
DO  580  1=1,4 

XS2( I , 1)=TEMP1( 1,1) 

580  CONTINUE 

XDIFF1=XT( 1,1)-XS1( 1, 1) 
YDIFF1=XT(3,1)-XS1(3,1) 

XDIFF2=XT( 1 , 1) -XS2( 1,1) 
YDIFF2=XT( 3 , 1) -XS2( 3,1) 

READ(2,*)N1 

READ(3,*)N2 

IF  (CASE. GE. 0.0)  GOTO  590 
N1=0. 0 
N2=0. 0 


590 


* 

600 


BRG1=RT0D*ATAN2( XDIFF1 ,YDIFF1)+N1 
IF  (BRG1. LT.  0.  0)  BRGl=BRGl+360 
BRG2=RT0D*ATAN2(XDIFF2,YDIFF2)+N2 
IF  (BRG2.LT.  0.0)  BRG2=BRG2+360 


WRITE(4,600)TIME,XT( 1,1) ,XT( 3, 1) ,XS1( 1, 1) ,XS1(3,1) , 
BRG1,XS2( 1, 1) ,XS2(3, 1) ,BRG2 
FORMATC I4,8F9.  4) 


TIMEM1=TIME 

TIME=TIME+PER 


IF  (TIME.  GT.  END)  GOTO  620 


o  o  n  n  o 


610 


CONTINUE 


620  STOP 
END 


SUBROUTINE  FINDPHI(DT,PHI) 


C  DIMENSIONS  AND  DECLERATIONS 

REAL*4  PHI (4, 4) ,DT 


PHI( 1, 1)=1.  0 
PHI(1,2)=DT 
PHI(1,3)=0.  0 
PHI( 1 ,4)=0.  0 
PHI(2,1)=0.  0 
PHI(2,2)=1.  0 
PHI(2,3)=0.0 
PHI(2,4)=0.  0 
PHI(3,1)=0.  0 
PHI(3,2)=0.0 
PHI(3,3)=1.  0 
PHI(3,4)=DT 
PHI(4,1)=0.0 
PHI(4,2)=0.  0 
PHI(4,3)=0.  0 
PHI(4,4)=1.  0 

RETURN 

END 

SUBROUTINE  MATMUL(A,B ,L,M,N,C) 

**^M-irM***k*******MMA*****1>**A**-irk1rk*ic**irkirkir*1rk**1rk-ir1t-iririrkil:*k**k-K1' 

THIS  ROUTINE  MULTIPLIES  TWO  MATRICES  TOGETHER 
0  C(L,N)  =  A(L,M)  *  B(M,N) 

■klricicMrlritrlrlt  irkirMrlrlrkie  Ar/r/ttefc  icttick  A  ^iW'Jr^jlntTHnlr******************  *******  **  * 

DIMENSIONS  AND  DECLARATIONS 
REAL*4  A(L,M) ,B(M,N) ,C(L,N) 

DO  10  1=1, L 
DO  10  J=1,N 
C(I,J)=0.  0 
10  CONTINUE 

DO  100  1=  1,L 
DO  100  J=  1,N 
DO  100  K=  1,M 

C(I,J)  =  C(I,J)  +  A(I,K)*B(K,J) 

100  CONTINUE 

RETURN 

END 
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